Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Replace Speeds with Velocities (#8479)

Browse Source 
I left "free speed" alone since that's the technical term for it. In
general, velocity is a vector quantity, and speed is a magnitude (i.e.,
a strictly positive value).

This PR also replaces the speed verbiage in MotorController with duty
cycle.

Fixes #8423.
This commit is contained in:
Tyler Veness
2026-03-06 14:19:15 -08:00
committed by GitHub
parent 1e39f39128
commit 9bd9656871
594 changed files with 8073 additions and 7875 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp
View File

@@ -26,8 +26,8 @@ void testFollowTrajectory(
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -86,12 +86,12 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
leftDistance += wheelVelocities.left * dt;
rightDistance += wheelVelocities.right * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -181,8 +181,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -226,8 +226,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

20
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -26,8 +26,8 @@ void testFollowTrajectory(
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -85,12 +85,12 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
leftDistance += wheelVelocities.left * dt;
rightDistance += wheelVelocities.right * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -174,8 +174,8 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -218,8 +218,8 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

24
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp
View File

@@ -21,8 +21,8 @@ void testFollowTrajectory(
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -79,14 +79,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -175,8 +175,8 @@ TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -220,8 +220,8 @@ TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

24
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -21,8 +21,8 @@ void testFollowTrajectory(
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -78,14 +78,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -169,8 +169,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -214,8 +214,8 @@ TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

21
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
View File

@@ -22,8 +22,8 @@ void testFollowTrajectory(
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator3d<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -81,13 +81,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
auto moduleVelocities =
kinematics.ToSwerveModuleVelocities(chassisVelocities);
for (size_t i = 0; i < 4; i++) {
positions[i].distance += moduleStates[i].speed * dt;
positions[i].angle = moduleStates[i].angle;
positions[i].distance += moduleVelocities[i].velocity * dt;
positions[i].angle = moduleVelocities[i].angle;
}
auto xhat = estimator.UpdateWithTime(
@@ -180,8 +181,8 @@ TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -229,8 +230,8 @@ TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

21
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -22,8 +22,8 @@ void testFollowTrajectory(
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -79,13 +79,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
auto moduleVelocities =
kinematics.ToSwerveModuleVelocities(chassisVelocities);
for (size_t i = 0; i < 4; i++) {
positions[i].distance += moduleStates[i].speed * dt;
positions[i].angle = moduleStates[i].angle;
positions[i].distance += moduleVelocities[i].velocity * dt;
positions[i].angle = moduleVelocities[i].angle;
}
auto xhat = estimator.UpdateWithTime(
@@ -173,8 +174,8 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -222,8 +223,8 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

103
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
View File

@@ -1,103 +0,0 @@
// 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/ChassisSpeeds.hpp"
#include <cmath>
#include <gtest/gtest.h>
static constexpr double kEpsilon = 1E-9;
TEST(ChassisSpeedsTest, Discretize) {
constexpr wpi::math::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s};
constexpr wpi::units::second_t duration = 1_s;
constexpr wpi::units::second_t dt = 10_ms;
const auto speeds = target.Discretize(duration);
const wpi::math::Twist2d twist{speeds.vx * dt, speeds.vy * dt,
speeds.omega * dt};
wpi::math::Pose2d pose;
for (wpi::units::second_t time = 0_s; time < duration; time += dt) {
pose = pose + twist.Exp();
}
EXPECT_NEAR((target.vx * duration).value(), pose.X().value(), kEpsilon);
EXPECT_NEAR((target.vy * duration).value(), pose.Y().value(), kEpsilon);
EXPECT_NEAR((target.omega * duration).value(),
pose.Rotation().Radians().value(), kEpsilon);
}
TEST(ChassisSpeedsTest, ToRobotRelative) {
const auto chassisSpeeds =
wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
-90.0_deg);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, ToFieldRelative) {
const auto chassisSpeeds =
wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(
45.0_deg);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, Plus) {
const wpi::math::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisSpeeds result = left + right;
EXPECT_NEAR(3.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(2.0, result.vy.value(), kEpsilon);
EXPECT_NEAR(1.0, result.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, Minus) {
const wpi::math::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisSpeeds result = left - right;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(0, result.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, result.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, UnaryMinus) {
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds result = -speeds;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon);
EXPECT_NEAR(-0.75, result.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, Multiplication) {
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds result = speeds * 2;
EXPECT_NEAR(2.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, result.vy.value(), kEpsilon);
EXPECT_NEAR(1.5, result.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, Division) {
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds result = speeds / 2;
EXPECT_NEAR(0.5, result.vx.value(), kEpsilon);
EXPECT_NEAR(0.25, result.vy.value(), kEpsilon);
EXPECT_NEAR(0.375, result.omega.value(), kEpsilon);
}

106
wpimath/src/test/native/cpp/kinematics/ChassisVelocitiesTest.cpp Normal file
View File

@@ -0,0 +1,106 @@
// 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/ChassisVelocities.hpp"
#include <cmath>
#include <gtest/gtest.h>
static constexpr double kEpsilon = 1E-9;
TEST(ChassisVelocitiesTest, Discretize) {
constexpr wpi::math::ChassisVelocities target{1_mps, 0_mps, 0.5_rad_per_s};
constexpr wpi::units::second_t duration = 1_s;
constexpr wpi::units::second_t dt = 10_ms;
const auto velocities = target.Discretize(duration);
const wpi::math::Twist2d twist{velocities.vx * dt, velocities.vy * dt,
velocities.omega * dt};
wpi::math::Pose2d pose;
for (wpi::units::second_t time = 0_s; time < duration; time += dt) {
pose = pose + twist.Exp();
}
EXPECT_NEAR((target.vx * duration).value(), pose.X().value(), kEpsilon);
EXPECT_NEAR((target.vy * duration).value(), pose.Y().value(), kEpsilon);
EXPECT_NEAR((target.omega * duration).value(),
pose.Rotation().Radians().value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, ToRobotRelative) {
const auto chassisVelocities =
wpi::math::ChassisVelocities{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
-90.0_deg);
EXPECT_NEAR(0.0, chassisVelocities.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisVelocities.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisVelocities.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, ToFieldRelative) {
const auto chassisVelocities =
wpi::math::ChassisVelocities{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(
45.0_deg);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisVelocities.vx.value(), kEpsilon);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisVelocities.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisVelocities.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, Plus) {
const wpi::math::ChassisVelocities left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisVelocities right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisVelocities result = left + right;
EXPECT_NEAR(3.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(2.0, result.vy.value(), kEpsilon);
EXPECT_NEAR(1.0, result.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, Minus) {
const wpi::math::ChassisVelocities left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisVelocities right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisVelocities result = left - right;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(0, result.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, result.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, UnaryMinus) {
const wpi::math::ChassisVelocities velocities{1.0_mps, 0.5_mps,
0.75_rad_per_s};
const wpi::math::ChassisVelocities result = -velocities;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon);
EXPECT_NEAR(-0.75, result.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, Multiplication) {
const wpi::math::ChassisVelocities velocities{1.0_mps, 0.5_mps,
0.75_rad_per_s};
const wpi::math::ChassisVelocities result = velocities * 2;
EXPECT_NEAR(2.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, result.vy.value(), kEpsilon);
EXPECT_NEAR(1.5, result.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, Division) {
const wpi::math::ChassisVelocities velocities{1.0_mps, 0.5_mps,
0.75_rad_per_s};
const wpi::math::ChassisVelocities result = velocities / 2;
EXPECT_NEAR(0.5, result.vx.value(), kEpsilon);
EXPECT_NEAR(0.25, result.vy.value(), kEpsilon);
EXPECT_NEAR(0.375, result.omega.value(), kEpsilon);
}

61
wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
View File

@@ -9,7 +9,7 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/units/angular_velocity.hpp"
#include "wpi/units/length.hpp"
#include "wpi/units/velocity.hpp"
@@ -20,62 +20,67 @@ static constexpr double kEpsilon = 1E-9;
TEST(DifferentialDriveKinematicsTest, InverseKinematicsFromZero) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds;
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
const ChassisVelocities chassisVelocities;
const auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
EXPECT_NEAR(wheelSpeeds.left.value(), 0, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), 0, kEpsilon);
EXPECT_NEAR(wheelVelocities.left.value(), 0, kEpsilon);
EXPECT_NEAR(wheelVelocities.right.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds;
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
const DifferentialDriveWheelVelocities wheelVelocities;
const auto chassisVelocities =
kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
const ChassisVelocities chassisVelocities{3.0_mps, 0_mps, 0_rad_per_s};
const auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
EXPECT_NEAR(wheelSpeeds.left.value(), 3, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), 3, kEpsilon);
EXPECT_NEAR(wheelVelocities.left.value(), 3, kEpsilon);
EXPECT_NEAR(wheelVelocities.right.value(), 3, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
const DifferentialDriveWheelVelocities wheelVelocities{3.0_mps, 3.0_mps};
const auto chassisVelocities =
kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(chassisSpeeds.vx.value(), 3, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 3, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds{
const ChassisVelocities chassisVelocities{
0.0_mps, 0.0_mps, wpi::units::radians_per_second_t{std::numbers::pi}};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
const auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * std::numbers::pi, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * std::numbers::pi, kEpsilon);
EXPECT_NEAR(wheelVelocities.left.value(), -0.381 * std::numbers::pi,
kEpsilon);
EXPECT_NEAR(wheelVelocities.right.value(), +0.381 * std::numbers::pi,
kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{
const DifferentialDriveWheelVelocities wheelVelocities{
wpi::units::meters_per_second_t{+0.381 * std::numbers::pi},
wpi::units::meters_per_second_t{-0.381 * std::numbers::pi}};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
const auto chassisVelocities =
kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), -std::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), -std::numbers::pi, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForZeros) {

54
wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp
View File

@@ -1,54 +0,0 @@
// 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/DifferentialDriveWheelSpeeds.hpp"
#include <gtest/gtest.h>
TEST(DifferentialDriveWheelSpeedsTest, Plus) {
const wpi::math::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds right{2.0_mps, 1.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = left + right;
EXPECT_EQ(3.0, result.left.value());
EXPECT_EQ(2.0, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, Minus) {
const wpi::math::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds right{2.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = left - right;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(0, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, UnaryMinus) {
const wpi::math::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = -speeds;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(-0.5, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, Multiplication) {
const wpi::math::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = speeds * 2;
EXPECT_EQ(2.0, result.left.value());
EXPECT_EQ(1.0, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, Division) {
const wpi::math::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = speeds / 2;
EXPECT_EQ(0.5, result.left.value());
EXPECT_EQ(0.25, result.right.value());
}

57
wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelVelocitiesTest.cpp Normal file
View File

@@ -0,0 +1,57 @@
// 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/DifferentialDriveWheelVelocities.hpp"
#include <gtest/gtest.h>
TEST(DifferentialDriveWheelVelocitiesTest, Plus) {
const wpi::math::DifferentialDriveWheelVelocities left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities right{2.0_mps, 1.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = left + right;
EXPECT_EQ(3.0, result.left.value());
EXPECT_EQ(2.0, result.right.value());
}
TEST(DifferentialDriveWheelVelocitiesTest, Minus) {
const wpi::math::DifferentialDriveWheelVelocities left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities right{2.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = left - right;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(0, result.right.value());
}
TEST(DifferentialDriveWheelVelocitiesTest, UnaryMinus) {
const wpi::math::DifferentialDriveWheelVelocities velocities{1.0_mps,
0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = -velocities;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(-0.5, result.right.value());
}
TEST(DifferentialDriveWheelVelocitiesTest, Multiplication) {
const wpi::math::DifferentialDriveWheelVelocities velocities{1.0_mps,
0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = velocities * 2;
EXPECT_EQ(2.0, result.left.value());
EXPECT_EQ(1.0, result.right.value());
}
TEST(DifferentialDriveWheelVelocitiesTest, Division) {
const wpi::math::DifferentialDriveWheelVelocities velocities{1.0_mps,
0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = velocities / 2;
EXPECT_EQ(0.5, result.left.value());
EXPECT_EQ(0.25, result.right.value());
}

170
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -24,22 +24,22 @@ class MecanumDriveKinematicsTest : public ::testing::Test {
};
TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
ChassisVelocities velocities{5_mps, 0_mps, 0_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities);
EXPECT_NEAR(5.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(5.0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(5.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 5_mps, 5_mps, 5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{5_mps, 5_mps, 5_mps, 5_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(5.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(5.0, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
@@ -52,22 +52,22 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
}
TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
ChassisVelocities velocities{0_mps, 4_mps, 0_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities);
EXPECT_NEAR(-4.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(4.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(4.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(-4.0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(4.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(4.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(-4.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 5_mps, 5_mps, -5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{-5_mps, 5_mps, 5_mps, -5_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(5.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(5.0, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) {
@@ -80,24 +80,24 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) {
}
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
ChassisVelocities velocities{
0_mps, 0_mps, wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(-150.79644737, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(-150.79644737, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-150.79644737_mps, 150.79644737_mps,
-150.79644737_mps, 150.79644737_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{
-150.79644737_mps, 150.79644737_mps, -150.79644737_mps, 150.79644737_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(2 * std::numbers::pi, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(2 * std::numbers::pi, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematicsWithDeltas) {
@@ -111,24 +111,24 @@ TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematicsWithDeltas) {
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
ChassisVelocities velocities{2_mps, 3_mps, 1_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities);
EXPECT_NEAR(-25.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(29.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-19.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(23.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(-25.0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(29.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(-19.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(23.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps,
-13.435_mps, 16.26_mps};
MecanumDriveWheelVelocities wheelVelocities{-17.677670_mps, 20.506097_mps,
-13.435_mps, 16.26_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(1.41335, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(2.1221, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(1.41335, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(2.1221, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -144,23 +144,23 @@ TEST_F(MecanumDriveKinematicsTest,
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
ChassisVelocities velocities{0_mps, 0_mps, 1_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities, m_fl);
EXPECT_NEAR(0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(24.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-24.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(48.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(24.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(-24.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(48.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps,
33.941_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{0_mps, 16.971_mps, -16.971_mps,
33.941_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(8.48525, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(-8.48525, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(8.48525, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(-8.48525, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -175,24 +175,24 @@ TEST_F(MecanumDriveKinematicsTest,
TEST_F(MecanumDriveKinematicsTest,
OffCenterTranslationRotationInverseKinematics) {
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
ChassisVelocities velocities{5_mps, 2_mps, 1_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities, m_fl);
EXPECT_NEAR(3.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(31.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-17.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(51.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(3.0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(31.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(-17.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(51.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
OffCenterTranslationRotationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{2.12_mps, 21.92_mps, -12.02_mps,
36.06_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{2.12_mps, 21.92_mps, -12.02_mps,
36.06_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(12.02, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(-7.07, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(12.02, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(-7.07, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -206,27 +206,29 @@ TEST_F(MecanumDriveKinematicsTest,
}
TEST_F(MecanumDriveKinematicsTest, Desaturate) {
auto wheelSpeeds =
MecanumDriveWheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps}.Desaturate(5.5_mps);
auto wheelVelocities =
MecanumDriveWheelVelocities{5_mps, 6_mps, 4_mps, 7_mps}.Desaturate(
5.5_mps);
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(wheelSpeeds.frontLeft.value(), 5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.frontLeft.value(), 5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.rearRight.value(), 7.0 * kFactor, 1E-9);
}
TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) {
auto wheelSpeeds =
MecanumDriveWheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps}.Desaturate(5.5_mps);
TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeVelocities) {
auto wheelVelocities =
MecanumDriveWheelVelocities{-5_mps, 6_mps, 4_mps, -7_mps}.Desaturate(
5.5_mps);
constexpr double kFactor = 5.5 / 7.0;
EXPECT_NEAR(wheelSpeeds.frontLeft.value(), -5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.value(), -7.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.frontLeft.value(), -5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.rearRight.value(), -7.0 * kFactor, 1E-9);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineInverseAccelerations) {

36
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp
View File

@@ -131,19 +131,19 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
while (t < trajectory.TotalTime()) {
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
auto wheelVelocities = kinematics.ToWheelVelocities(
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelVelocities.frontLeft += distribution(generator) * 0.1_mps;
wheelVelocities.frontRight += distribution(generator) * 0.1_mps;
wheelVelocities.rearLeft += distribution(generator) * 0.1_mps;
wheelVelocities.rearRight += distribution(generator) * 0.1_mps;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
auto xhat = odometry.Update(
wpi::math::Rotation3d{
@@ -198,20 +198,20 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
while (t < trajectory.TotalTime()) {
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
auto wheelVelocities = kinematics.ToWheelVelocities(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
0_rad_per_s});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelVelocities.frontLeft += distribution(generator) * 0.1_mps;
wheelVelocities.frontRight += distribution(generator) * 0.1_mps;
wheelVelocities.rearLeft += distribution(generator) * 0.1_mps;
wheelVelocities.rearRight += distribution(generator) * 0.1_mps;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
auto xhat = odometry.Update(
wpi::math::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},

36
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -110,19 +110,19 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
while (t < trajectory.TotalTime()) {
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
auto wheelVelocities = kinematics.ToWheelVelocities(
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelVelocities.frontLeft += distribution(generator) * 0.1_mps;
wheelVelocities.frontRight += distribution(generator) * 0.1_mps;
wheelVelocities.rearLeft += distribution(generator) * 0.1_mps;
wheelVelocities.rearRight += distribution(generator) * 0.1_mps;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
auto xhat = odometry.Update(
groundTruthState.pose.Rotation() +
@@ -176,20 +176,20 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
while (t < trajectory.TotalTime()) {
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
auto wheelVelocities = kinematics.ToWheelVelocities(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
0_rad_per_s});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelVelocities.frontLeft += distribution(generator) * 0.1_mps;
wheelVelocities.frontRight += distribution(generator) * 0.1_mps;
wheelVelocities.rearLeft += distribution(generator) * 0.1_mps;
wheelVelocities.rearRight += distribution(generator) * 0.1_mps;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
auto xhat = odometry.Update(
wpi::math::Rotation2d{distribution(generator) * 0.05_rad},

71
wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp
View File

@@ -1,71 +0,0 @@
// 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/MecanumDriveWheelSpeeds.hpp"
#include <gtest/gtest.h>
TEST(MecanumDriveWheelSpeedsTest, Plus) {
const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps,
1.0_mps};
const wpi::math::MecanumDriveWheelSpeeds result = left + right;
EXPECT_EQ(3.0, result.frontLeft.value());
EXPECT_EQ(2.0, result.frontRight.value());
EXPECT_EQ(2.5, result.rearLeft.value());
EXPECT_EQ(2.5, result.rearRight.value());
}
TEST(MecanumDriveWheelSpeedsTest, Minus) {
const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps,
1.0_mps};
const wpi::math::MecanumDriveWheelSpeeds result = left - right;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-1.0, result.frontRight.value());
EXPECT_EQ(1.5, result.rearLeft.value());
EXPECT_EQ(0.5, result.rearRight.value());
}
TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds result = -speeds;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-0.5, result.frontRight.value());
EXPECT_EQ(-2.0, result.rearLeft.value());
EXPECT_EQ(-1.5, result.rearRight.value());
}
TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds result = speeds * 2;
EXPECT_EQ(2.0, result.frontLeft.value());
EXPECT_EQ(1.0, result.frontRight.value());
EXPECT_EQ(4.0, result.rearLeft.value());
EXPECT_EQ(3.0, result.rearRight.value());
}
TEST(MecanumDriveWheelSpeedsTest, Division) {
const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds result = speeds / 2;
EXPECT_EQ(0.5, result.frontLeft.value());
EXPECT_EQ(0.25, result.frontRight.value());
EXPECT_EQ(1.0, result.rearLeft.value());
EXPECT_EQ(0.75, result.rearRight.value());
}

71
wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelVelocitiesTest.cpp Normal file
View File

@@ -0,0 +1,71 @@
// 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/MecanumDriveWheelVelocities.hpp"
#include <gtest/gtest.h>
TEST(MecanumDriveWheelVelocitiesTest, Plus) {
const wpi::math::MecanumDriveWheelVelocities left{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelVelocities right{2.0_mps, 1.5_mps, 0.5_mps,
1.0_mps};
const wpi::math::MecanumDriveWheelVelocities result = left + right;
EXPECT_EQ(3.0, result.frontLeft.value());
EXPECT_EQ(2.0, result.frontRight.value());
EXPECT_EQ(2.5, result.rearLeft.value());
EXPECT_EQ(2.5, result.rearRight.value());
}
TEST(MecanumDriveWheelVelocitiesTest, Minus) {
const wpi::math::MecanumDriveWheelVelocities left{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelVelocities right{2.0_mps, 1.5_mps, 0.5_mps,
1.0_mps};
const wpi::math::MecanumDriveWheelVelocities result = left - right;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-1.0, result.frontRight.value());
EXPECT_EQ(1.5, result.rearLeft.value());
EXPECT_EQ(0.5, result.rearRight.value());
}
TEST(MecanumDriveWheelVelocitiesTest, UnaryMinus) {
const wpi::math::MecanumDriveWheelVelocities velocities{1.0_mps, 0.5_mps,
2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelVelocities result = -velocities;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-0.5, result.frontRight.value());
EXPECT_EQ(-2.0, result.rearLeft.value());
EXPECT_EQ(-1.5, result.rearRight.value());
}
TEST(MecanumDriveWheelVelocitiesTest, Multiplication) {
const wpi::math::MecanumDriveWheelVelocities velocities{1.0_mps, 0.5_mps,
2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelVelocities result = velocities * 2;
EXPECT_EQ(2.0, result.frontLeft.value());
EXPECT_EQ(1.0, result.frontRight.value());
EXPECT_EQ(4.0, result.rearLeft.value());
EXPECT_EQ(3.0, result.rearRight.value());
}
TEST(MecanumDriveWheelVelocitiesTest, Division) {
const wpi::math::MecanumDriveWheelVelocities velocities{1.0_mps, 0.5_mps,
2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelVelocities result = velocities / 2;
EXPECT_EQ(0.5, result.frontLeft.value());
EXPECT_EQ(0.25, result.frontRight.value());
EXPECT_EQ(1.0, result.rearLeft.value());
EXPECT_EQ(0.75, result.rearRight.value());
}

229
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -26,14 +26,14 @@ class SwerveDriveKinematicsTest : public ::testing::Test {
};
TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};
ChassisVelocities velocities{5.0_mps, 0.0_mps, 0.0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleVelocities(velocities);
EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Radians().value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Radians().value(), 0.0, kEpsilon);
@@ -42,13 +42,14 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
SwerveModuleState state{5.0_mps, 0_deg};
SwerveModuleVelocity state{5.0_mps, 0_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
auto chassisVelocities =
m_kinematics.ToChassisVelocities(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
@@ -62,13 +63,13 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
ChassisVelocities velocities{0_mps, 5_mps, 0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleVelocities(velocities);
EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 90.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 90.0, kEpsilon);
@@ -77,12 +78,13 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
SwerveModuleState state{5_mps, 90_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
SwerveModuleVelocity state{5_mps, 90_deg};
auto chassisVelocities =
m_kinematics.ToChassisVelocities(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematicsWithDeltas) {
@@ -96,14 +98,14 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematicsWithDeltas) {
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
ChassisVelocities velocities{
0_mps, 0_mps, wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleVelocities(velocities);
EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(br.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 106.63, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 106.63, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 106.63, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 106.63, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
@@ -112,15 +114,16 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
ChassisSpeeds speeds{0_mps, 0_mps,
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
m_kinematics.ToSwerveModuleStates(speeds);
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
ChassisVelocities velocities{
0_mps, 0_mps, wpi::units::radians_per_second_t{2 * std::numbers::pi}};
m_kinematics.ToSwerveModuleVelocities(velocities);
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleVelocities(ChassisVelocities{});
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
@@ -134,12 +137,12 @@ TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) {
Rotation2d br = {270_deg};
m_kinematics.ResetHeadings(fl, fr, bl, br);
auto [flMod, frMod, blMod, brMod] =
m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
m_kinematics.ToSwerveModuleVelocities(ChassisVelocities{});
EXPECT_NEAR(flMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(frMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(blMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(brMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(flMod.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(frMod.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(blMod.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(brMod.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(flMod.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(frMod.angle.Degrees().value(), 90.0, kEpsilon);
@@ -148,16 +151,16 @@ TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) {
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
SwerveModuleState fl{106.629_mps, 135_deg};
SwerveModuleState fr{106.629_mps, 45_deg};
SwerveModuleState bl{106.629_mps, -135_deg};
SwerveModuleState br{106.629_mps, -45_deg};
SwerveModuleVelocity fl{106.629_mps, 135_deg};
SwerveModuleVelocity fr{106.629_mps, 45_deg};
SwerveModuleVelocity bl{106.629_mps, -135_deg};
SwerveModuleVelocity br{106.629_mps, -45_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
auto chassisVelocities = m_kinematics.ToChassisVelocities(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 2 * std::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematicsWithDeltas) {
@@ -174,14 +177,15 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematicsWithDeltas) {
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
ChassisVelocities velocities{
0_mps, 0_mps, wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleVelocities(velocities, m_fl);
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 150.796, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 150.796, kEpsilon);
EXPECT_NEAR(br.speed.value(), 213.258, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 150.796, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 150.796, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 213.258, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 0.0, kEpsilon);
@@ -190,16 +194,16 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
SwerveModuleState fl{0.0_mps, 0_deg};
SwerveModuleState fr{150.796_mps, 0_deg};
SwerveModuleState bl{150.796_mps, -90_deg};
SwerveModuleState br{213.258_mps, -45_deg};
SwerveModuleVelocity fl{0.0_mps, 0_deg};
SwerveModuleVelocity fr{150.796_mps, 0_deg};
SwerveModuleVelocity bl{150.796_mps, -90_deg};
SwerveModuleVelocity br{213.258_mps, -45_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
auto chassisVelocities = m_kinematics.ToChassisVelocities(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.value(), 75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), -75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 75.398, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), -75.398, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 2 * std::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -218,14 +222,14 @@ TEST_F(SwerveDriveKinematicsTest,
TEST_F(SwerveDriveKinematicsTest,
OffCenterCORRotationAndTranslationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleStates(speeds, Translation2d{24_m, 0_m});
ChassisVelocities velocities{0_mps, 3.0_mps, 1.5_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleVelocities(
velocities, Translation2d{24_m, 0_m});
EXPECT_NEAR(fl.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 54.08, kEpsilon);
EXPECT_NEAR(br.speed.value(), 54.08, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 23.43, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 23.43, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 54.08, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 54.08, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), -140.19, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), -39.81, kEpsilon);
@@ -235,16 +239,16 @@ TEST_F(SwerveDriveKinematicsTest,
TEST_F(SwerveDriveKinematicsTest,
OffCenterCORRotationAndTranslationForwardKinematics) {
SwerveModuleState fl{23.43_mps, -140.19_deg};
SwerveModuleState fr{23.43_mps, -39.81_deg};
SwerveModuleState bl{54.08_mps, -109.44_deg};
SwerveModuleState br{54.08_mps, -70.56_deg};
SwerveModuleVelocity fl{23.43_mps, -140.19_deg};
SwerveModuleVelocity fr{23.43_mps, -39.81_deg};
SwerveModuleVelocity bl{54.08_mps, -109.44_deg};
SwerveModuleVelocity br{54.08_mps, -70.56_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
auto chassisVelocities = m_kinematics.ToChassisVelocities(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), -33.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), -33.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 1.5, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -262,53 +266,54 @@ TEST_F(SwerveDriveKinematicsTest,
}
TEST_F(SwerveDriveKinematicsTest, Desaturate) {
SwerveModuleState state1{5.0_mps, 0_deg};
SwerveModuleState state2{6.0_mps, 0_deg};
SwerveModuleState state3{4.0_mps, 0_deg};
SwerveModuleState state4{7.0_mps, 0_deg};
SwerveModuleVelocity state1{5.0_mps, 0_deg};
SwerveModuleVelocity state2{6.0_mps, 0_deg};
SwerveModuleVelocity state3{4.0_mps, 0_deg};
SwerveModuleVelocity state4{7.0_mps, 0_deg};
wpi::util::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 5.5_mps);
wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 5.5_mps);
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[0].velocity.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].velocity.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].velocity.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].velocity.value(), 7.0 * kFactor, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
SwerveModuleState state1{5.0_mps, 0_deg};
SwerveModuleState state2{6.0_mps, 0_deg};
SwerveModuleState state3{4.0_mps, 0_deg};
SwerveModuleState state4{7.0_mps, 0_deg};
SwerveModuleVelocity state1{5.0_mps, 0_deg};
SwerveModuleVelocity state2{6.0_mps, 0_deg};
SwerveModuleVelocity state3{4.0_mps, 0_deg};
SwerveModuleVelocity state4{7.0_mps, 0_deg};
wpi::util::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
&arr, m_kinematics.ToChassisSpeeds(arr), 5.5_mps, 5.5_mps, 3.5_rad_per_s);
wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelVelocities(
&arr, m_kinematics.ToChassisVelocities(arr), 5.5_mps, 5.5_mps,
3.5_rad_per_s);
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[0].velocity.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].velocity.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].velocity.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].velocity.value(), 7.0 * kFactor, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) {
SwerveModuleState state1{1.0_mps, 0_deg};
SwerveModuleState state2{1.0_mps, 0_deg};
SwerveModuleState state3{-2.0_mps, 0_deg};
SwerveModuleState state4{-2.0_mps, 0_deg};
TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeVelocity) {
SwerveModuleVelocity state1{1.0_mps, 0_deg};
SwerveModuleVelocity state2{1.0_mps, 0_deg};
SwerveModuleVelocity state3{-2.0_mps, 0_deg};
SwerveModuleVelocity state4{-2.0_mps, 0_deg};
wpi::util::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 1.0_mps);
wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 1.0_mps);
EXPECT_NEAR(arr[0].speed.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), -1.0, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), -1.0, kEpsilon);
EXPECT_NEAR(arr[0].velocity.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[1].velocity.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[2].velocity.value(), -1.0, kEpsilon);
EXPECT_NEAR(arr[3].velocity.value(), -1.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) {

18
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp
View File

@@ -127,19 +127,19 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
while (t < trajectory.TotalTime()) {
Trajectory::State groundTruthState = trajectory.Sample(t);
auto moduleStates = kinematics.ToSwerveModuleStates(
auto moduleVelocities = kinematics.ToSwerveModuleVelocities(
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
fl.distance += moduleStates[0].speed * dt;
fr.distance += moduleStates[1].speed * dt;
bl.distance += moduleStates[2].speed * dt;
br.distance += moduleStates[3].speed * dt;
fl.distance += moduleVelocities[0].velocity * dt;
fr.distance += moduleVelocities[1].velocity * dt;
bl.distance += moduleVelocities[2].velocity * dt;
br.distance += moduleVelocities[3].velocity * dt;
fl.angle = moduleStates[0].angle;
fr.angle = moduleStates[1].angle;
bl.angle = moduleStates[2].angle;
br.angle = moduleStates[3].angle;
fl.angle = moduleVelocities[0].angle;
fr.angle = moduleVelocities[1].angle;
bl.angle = moduleVelocities[2].angle;
br.angle = moduleVelocities[3].angle;
auto xhat = odometry.Update(
wpi::math::Rotation3d{

18
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -104,19 +104,19 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
while (t < trajectory.TotalTime()) {
Trajectory::State groundTruthState = trajectory.Sample(t);
auto moduleStates = kinematics.ToSwerveModuleStates(
auto moduleVelocities = kinematics.ToSwerveModuleVelocities(
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
fl.distance += moduleStates[0].speed * dt;
fr.distance += moduleStates[1].speed * dt;
bl.distance += moduleStates[2].speed * dt;
br.distance += moduleStates[3].speed * dt;
fl.distance += moduleVelocities[0].velocity * dt;
fr.distance += moduleVelocities[1].velocity * dt;
bl.distance += moduleVelocities[2].velocity * dt;
br.distance += moduleVelocities[3].velocity * dt;
fl.angle = moduleStates[0].angle;
fr.angle = moduleStates[1].angle;
bl.angle = moduleStates[2].angle;
br.angle = moduleStates[3].angle;
fl.angle = moduleVelocities[0].angle;
fr.angle = moduleVelocities[1].angle;
bl.angle = moduleVelocities[2].angle;
br.angle = moduleVelocities[3].angle;
auto xhat = odometry.Update(
groundTruthState.pose.Rotation() +

86
wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp → wpimath/src/test/native/cpp/kinematics/SwerveModuleVelocityTest.cpp
View File

@@ -2,7 +2,7 @@
// 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/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include <gtest/gtest.h>
@@ -10,135 +10,135 @@
static constexpr double kEpsilon = 1E-9;
TEST(SwerveModuleStateTest, Optimize) {
TEST(SwerveModuleVelocityTest, Optimize) {
wpi::math::Rotation2d angleA{45_deg};
wpi::math::SwerveModuleState refA{-2_mps, 180_deg};
wpi::math::SwerveModuleVelocity refA{-2_mps, 180_deg};
refA.Optimize(angleA);
EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 0.0, kEpsilon);
wpi::math::Rotation2d angleB{-50_deg};
wpi::math::SwerveModuleState refB{4.7_mps, 41_deg};
wpi::math::SwerveModuleVelocity refB{4.7_mps, 41_deg};
refB.Optimize(angleB);
EXPECT_NEAR(refB.speed.value(), -4.7, kEpsilon);
EXPECT_NEAR(refB.velocity.value(), -4.7, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), -139.0, kEpsilon);
}
TEST(SwerveModuleStateTest, NoOptimize) {
TEST(SwerveModuleVelocityTest, NoOptimize) {
wpi::math::Rotation2d angleA{0_deg};
wpi::math::SwerveModuleState refA{2_mps, 89_deg};
wpi::math::SwerveModuleVelocity refA{2_mps, 89_deg};
refA.Optimize(angleA);
EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 89.0, kEpsilon);
wpi::math::Rotation2d angleB{0_deg};
wpi::math::SwerveModuleState refB{-2_mps, -2_deg};
wpi::math::SwerveModuleVelocity refB{-2_mps, -2_deg};
refB.Optimize(angleB);
EXPECT_NEAR(refB.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refB.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), -2.0, kEpsilon);
}
TEST(SwerveModuleStateTest, CosineScaling) {
TEST(SwerveModuleVelocityTest, CosineScaling) {
wpi::math::Rotation2d angleA{0_deg};
wpi::math::SwerveModuleState refA{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refA{2_mps, 45_deg};
refA.CosineScale(angleA);
EXPECT_NEAR(refA.speed.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refA.velocity.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleB{45_deg};
wpi::math::SwerveModuleState refB{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refB{2_mps, 45_deg};
refB.CosineScale(angleB);
EXPECT_NEAR(refB.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refB.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleC{-45_deg};
wpi::math::SwerveModuleState refC{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refC{2_mps, 45_deg};
refC.CosineScale(angleC);
EXPECT_NEAR(refC.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refC.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refC.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleD{135_deg};
wpi::math::SwerveModuleState refD{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refD{2_mps, 45_deg};
refD.CosineScale(angleD);
EXPECT_NEAR(refD.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refD.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refD.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleE{-135_deg};
wpi::math::SwerveModuleState refE{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refE{2_mps, 45_deg};
refE.CosineScale(angleE);
EXPECT_NEAR(refE.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refE.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(refE.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleF{180_deg};
wpi::math::SwerveModuleState refF{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refF{2_mps, 45_deg};
refF.CosineScale(angleF);
EXPECT_NEAR(refF.speed.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refF.velocity.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refF.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleG{0_deg};
wpi::math::SwerveModuleState refG{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refG{-2_mps, 45_deg};
refG.CosineScale(angleG);
EXPECT_NEAR(refG.speed.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refG.velocity.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refG.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleH{45_deg};
wpi::math::SwerveModuleState refH{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refH{-2_mps, 45_deg};
refH.CosineScale(angleH);
EXPECT_NEAR(refH.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refH.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(refH.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleI{-45_deg};
wpi::math::SwerveModuleState refI{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refI{-2_mps, 45_deg};
refI.CosineScale(angleI);
EXPECT_NEAR(refI.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refI.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refI.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleJ{135_deg};
wpi::math::SwerveModuleState refJ{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refJ{-2_mps, 45_deg};
refJ.CosineScale(angleJ);
EXPECT_NEAR(refJ.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refJ.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refJ.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleK{-135_deg};
wpi::math::SwerveModuleState refK{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refK{-2_mps, 45_deg};
refK.CosineScale(angleK);
EXPECT_NEAR(refK.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refK.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refK.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleL{180_deg};
wpi::math::SwerveModuleState refL{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refL{-2_mps, 45_deg};
refL.CosineScale(angleL);
EXPECT_NEAR(refL.speed.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refL.velocity.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refL.angle.Degrees().value(), 45.0, kEpsilon);
}
TEST(SwerveModuleStateTest, Equality) {
wpi::math::SwerveModuleState state1{2_mps, 90_deg};
wpi::math::SwerveModuleState state2{2_mps, 90_deg};
TEST(SwerveModuleVelocityTest, Equality) {
wpi::math::SwerveModuleVelocity state1{2_mps, 90_deg};
wpi::math::SwerveModuleVelocity state2{2_mps, 90_deg};
EXPECT_EQ(state1, state2);
}
TEST(SwerveModuleStateTest, Inequality) {
wpi::math::SwerveModuleState state1{1_mps, 90_deg};
wpi::math::SwerveModuleState state2{2_mps, 90_deg};
wpi::math::SwerveModuleState state3{1_mps, 89_deg};
TEST(SwerveModuleVelocityTest, Inequality) {
wpi::math::SwerveModuleVelocity state1{1_mps, 90_deg};
wpi::math::SwerveModuleVelocity state2{2_mps, 90_deg};
wpi::math::SwerveModuleVelocity state3{1_mps, 89_deg};
EXPECT_NE(state1, state2);
EXPECT_NE(state1, state3);

8
wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp → wpimath/src/test/native/cpp/kinematics/proto/ChassisVelocitiesProtoTest.cpp
View File

@@ -4,18 +4,18 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace wpi::math;
namespace {
const ChassisSpeeds kExpectedData =
ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s};
const ChassisVelocities kExpectedData =
ChassisVelocities{2.29_mps, 2.2_mps, 0.3504_rad_per_s};
} // namespace
TEST(ChassisSpeedsProtoTest, Roundtrip) {
TEST(ChassisVelocitiesProtoTest, Roundtrip) {
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;

8
wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp → wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelVelocitiesProtoTest.cpp
View File

@@ -4,18 +4,18 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace wpi::math;
namespace {
const DifferentialDriveWheelSpeeds kExpectedData =
DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps};
const DifferentialDriveWheelVelocities kExpectedData =
DifferentialDriveWheelVelocities{1.74_mps, 35.04_mps};
} // namespace
TEST(DifferentialDriveWheelSpeedsProtoTest, Roundtrip) {
TEST(DifferentialDriveWheelVelocitiesProtoTest, Roundtrip) {
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;

8
wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp → wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelVelocitiesProtoTest.cpp
View File

@@ -4,18 +4,18 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace wpi::math;
namespace {
const MecanumDriveWheelSpeeds kExpectedData =
MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps};
const MecanumDriveWheelVelocities kExpectedData =
MecanumDriveWheelVelocities{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps};
} // namespace
TEST(MecanumDriveWheelSpeedsProtoTest, Roundtrip) {
TEST(MecanumDriveWheelVelocitiesProtoTest, Roundtrip) {
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;

10
wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp → wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleVelocityProtoTest.cpp
View File

@@ -4,18 +4,18 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace wpi::math;
namespace {
const SwerveModuleState kExpectedData =
SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}};
const SwerveModuleVelocity kExpectedData =
SwerveModuleVelocity{22.9_mps, Rotation2d{3.3_rad}};
} // namespace
TEST(SwerveModuleStateProtoTest, Roundtrip) {
TEST(SwerveModuleVelocityProtoTest, Roundtrip) {
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
@@ -23,6 +23,6 @@ TEST(SwerveModuleStateProtoTest, Roundtrip) {
auto unpacked_data = message.Unpack(buf);
ASSERT_TRUE(unpacked_data.has_value());
EXPECT_EQ(kExpectedData.speed.value(), unpacked_data->speed.value());
EXPECT_EQ(kExpectedData.velocity.value(), unpacked_data->velocity.value());
EXPECT_EQ(kExpectedData.angle, unpacked_data->angle);
}

12
wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp → wpimath/src/test/native/cpp/kinematics/struct/ChassisVelocitiesStructTest.cpp
View File

@@ -4,23 +4,23 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
using namespace wpi::math;
namespace {
using StructType = wpi::util::Struct<wpi::math::ChassisSpeeds>;
const ChassisSpeeds kExpectedData{
ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s}};
using StructType = wpi::util::Struct<wpi::math::ChassisVelocities>;
const ChassisVelocities kExpectedData{
ChassisVelocities{2.29_mps, 2.2_mps, 0.3504_rad_per_s}};
} // namespace
TEST(ChassisSpeedsStructTest, Roundtrip) {
TEST(ChassisVelocitiesStructTest, Roundtrip) {
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
StructType::Pack(buffer, kExpectedData);
ChassisSpeeds unpacked_data = StructType::Unpack(buffer);
ChassisVelocities unpacked_data = StructType::Unpack(buffer);
EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value());
EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value());

13
wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp → wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelVelocitiesStructTest.cpp
View File

@@ -4,23 +4,24 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
using namespace wpi::math;
namespace {
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds>;
const DifferentialDriveWheelSpeeds kExpectedData{
DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps}};
using StructType =
wpi::util::Struct<wpi::math::DifferentialDriveWheelVelocities>;
const DifferentialDriveWheelVelocities kExpectedData{
DifferentialDriveWheelVelocities{1.74_mps, 35.04_mps}};
} // namespace
TEST(DifferentialDriveWheelSpeedsStructTest, Roundtrip) {
TEST(DifferentialDriveWheelVelocitiesStructTest, Roundtrip) {
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
StructType::Pack(buffer, kExpectedData);
DifferentialDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer);
DifferentialDriveWheelVelocities unpacked_data = StructType::Unpack(buffer);
EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());

12
wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp → wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelVelocitiesStructTest.cpp
View File

@@ -4,23 +4,23 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
using namespace wpi::math;
namespace {
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds>;
const MecanumDriveWheelSpeeds kExpectedData{
MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}};
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelVelocities>;
const MecanumDriveWheelVelocities kExpectedData{
MecanumDriveWheelVelocities{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}};
} // namespace
TEST(MecanumDriveWheelSpeedsStructTest, Roundtrip) {
TEST(MecanumDriveWheelVelocitiesStructTest, Roundtrip) {
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
StructType::Pack(buffer, kExpectedData);
MecanumDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer);
MecanumDriveWheelVelocities unpacked_data = StructType::Unpack(buffer);
EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());

14
wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp → wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleVelocityStructTest.cpp
View File

@@ -4,24 +4,24 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
using namespace wpi::math;
namespace {
using StructType = wpi::util::Struct<wpi::math::SwerveModuleState>;
const SwerveModuleState kExpectedData{
SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}}};
using StructType = wpi::util::Struct<wpi::math::SwerveModuleVelocity>;
const SwerveModuleVelocity kExpectedData{
SwerveModuleVelocity{22.9_mps, Rotation2d{3.3_rad}}};
} // namespace
TEST(SwerveModuleStateStructTest, Roundtrip) {
TEST(SwerveModuleVelocityStructTest, Roundtrip) {
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
StructType::Pack(buffer, kExpectedData);
SwerveModuleState unpacked_data = StructType::Unpack(buffer);
SwerveModuleVelocity unpacked_data = StructType::Unpack(buffer);
EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value());
EXPECT_EQ(kExpectedData.velocity.value(), unpacked_data.velocity.value());
EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
}

6
wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
View File

@@ -30,10 +30,10 @@ TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
const Trajectory::State point = trajectory.Sample(time);
time += dt;
const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
point.velocity * point.curvature};
const ChassisVelocities chassisVelocities{point.velocity, 0_mps,
point.velocity * point.curvature};
auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
auto [left, right] = kinematics.ToWheelVelocities(chassisVelocities);
EXPECT_TRUE(left < maxVelocity + 0.05_mps);
EXPECT_TRUE(right < maxVelocity + 0.05_mps);

6
wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
View File

@@ -40,10 +40,10 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
const Trajectory::State point = trajectory.Sample(time);
time += dt;
const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
point.velocity * point.curvature};
const ChassisVelocities chassisVelocities{point.velocity, 0_mps,
point.velocity * point.curvature};
auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
auto [left, right] = kinematics.ToWheelVelocities(chassisVelocities);
auto acceleration = point.acceleration;
// Not really a strictly-correct test as we're using the chassis accel
// instead of the wheel accel, but much easier than doing it "properly" and

28
wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
View File

@@ -161,8 +161,8 @@ TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, TopSpeed) {
// Checks to make sure that it hits top velocity on long trajectories
TEST(ExponentialProfileTest, TopVelocity) {
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
@@ -175,19 +175,19 @@ TEST(ExponentialProfileTest, TopSpeed) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state;
wpi::units::meters_per_second_t maxSpeed = 0_mps;
wpi::units::meters_per_second_t maxVelocity = 0_mps;
for (int i = 0; i < 900; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
maxSpeed = wpi::units::math::max(state.velocity, maxSpeed);
maxVelocity = wpi::units::math::max(state.velocity, maxVelocity);
}
EXPECT_NEAR_UNITS(constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
EXPECT_NEAR_UNITS(constraints.MaxVelocity(), maxVelocity, 1e-5_mps);
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, TopSpeedBackward) {
// Checks to make sure that it hits top velocity on long trajectories
TEST(ExponentialProfileTest, TopVelocityBackward) {
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
@@ -200,19 +200,19 @@ TEST(ExponentialProfileTest, TopSpeedBackward) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state;
wpi::units::meters_per_second_t maxSpeed = 0_mps;
wpi::units::meters_per_second_t maxVelocity = 0_mps;
for (int i = 0; i < 900; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
maxSpeed = wpi::units::math::min(state.velocity, maxSpeed);
maxVelocity = wpi::units::math::min(state.velocity, maxVelocity);
}
EXPECT_NEAR_UNITS(-constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
EXPECT_NEAR_UNITS(-constraints.MaxVelocity(), maxVelocity, 1e-5_mps);
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, HighInitialSpeed) {
// Checks to make sure that it hits top velocity on long trajectories
TEST(ExponentialProfileTest, HighInitialVelocity) {
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
@@ -232,8 +232,8 @@ TEST(ExponentialProfileTest, HighInitialSpeed) {
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
// Checks to make sure that it hits top velocity on long trajectories
TEST(ExponentialProfileTest, HighInitialVelocityBackward) {
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};

4
wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
View File

@@ -105,8 +105,8 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed
TEST(TrapezoidProfileTest, TopSpeed) {
// Checks to make sure that it hits top velocity
TEST(TrapezoidProfileTest, TopVelocity) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{4_m, 0_mps};