Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -23,7 +23,7 @@ using namespace wpi::math;
TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
// Pick an unreasonably large kA to ensure the constraint has to do some work
SimpleMotorFeedforward<wpi::units::meter> feedforward{1_V, 1_V / 1_mps,
3_V / 1_mps_sq};
3_V / 1_mps_sq};
const DifferentialDriveKinematics kinematics{0.5_m};
const auto maxVoltage = 10_V;
@@ -64,7 +64,7 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
TEST(DifferentialDriveVoltageConstraintTest, HighCurvature) {
SimpleMotorFeedforward<wpi::units::meter> feedforward{1_V, 1_V / 1_mps,
3_V / 1_mps_sq};
3_V / 1_mps_sq};
// Large trackwidth - need to test with radius of curvature less than half of
// trackwidth
const DifferentialDriveKinematics kinematics{3_m};

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

@@ -26,8 +26,10 @@ TEST(EllipticalRegionConstraintTest, Constraint) {
bool exceededConstraintOutsideRegion = false;
for (auto& point : trajectory.States()) {
if (ellipse.Contains(point.pose.Translation())) {
EXPECT_TRUE(wpi::units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
} else if (wpi::units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
EXPECT_TRUE(wpi::units::math::abs(point.velocity) <
maxVelocity + 0.05_mps);
} else if (wpi::units::math::abs(point.velocity) >=
maxVelocity + 0.05_mps) {
exceededConstraintOutsideRegion = true;
}
}

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

@@ -33,13 +33,16 @@ static constexpr auto kA = 0.43277_V / 1_mps_sq;
EXPECT_NEAR_UNITS(val1, val2, eps); \
}
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State CheckDynamics(
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
CheckDynamics(
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile,
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints
constraints,
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints,
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward,
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State current,
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
current,
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal) {
auto next = profile.Calculate(kDt, current, goal);
auto signal = feedforward.Calculate(current.velocity, next.velocity);
@@ -49,13 +52,17 @@ wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State Check
}
TEST(ExponentialProfileTest, ReachesGoal) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state{0_m, 0_mps};
for (int i = 0; i < 450; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -66,19 +73,24 @@ TEST(ExponentialProfileTest, ReachesGoal) {
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
TEST(ExponentialProfileTest, PosContinuousUnderVelChange) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state{0_m, 0_mps};
for (int i = 0; i < 300; ++i) {
if (i == 150) {
constraints.maxInput = 9_V;
profile =
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>{
constraints};
}
state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -89,19 +101,24 @@ TEST(ExponentialProfileTest, PosContinuousUnderVelChange) {
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
TEST(ExponentialProfileTest, PosContinuousUnderVelChangeBackward) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{-10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state{0_m, 0_mps};
for (int i = 0; i < 300; ++i) {
if (i == 150) {
constraints.maxInput = 9_V;
profile =
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>{
constraints};
}
state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -111,13 +128,17 @@ TEST(ExponentialProfileTest, PosContinuousUnderVelChangeBackward) {
// There is some somewhat tricky code for dealing with going backwards
TEST(ExponentialProfileTest, Backwards) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state;
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{-10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state;
for (int i = 0; i < 400; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -126,13 +147,17 @@ TEST(ExponentialProfileTest, Backwards) {
}
TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{-10_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state{0_m, 0_mps};
for (int i = 0; i < 50; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -148,13 +173,17 @@ TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, TopSpeed) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{40_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state;
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{40_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state;
wpi::units::meters_per_second_t maxSpeed = 0_mps;
@@ -169,13 +198,17 @@ TEST(ExponentialProfileTest, TopSpeed) {
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, TopSpeedBackward) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-40_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state;
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{-40_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state;
wpi::units::meters_per_second_t maxSpeed = 0_mps;
@@ -190,13 +223,17 @@ TEST(ExponentialProfileTest, TopSpeedBackward) {
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, HighInitialSpeed) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{40_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 8_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{40_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state{0_m, 8_mps};
for (int i = 0; i < 900; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -207,13 +244,17 @@ TEST(ExponentialProfileTest, HighInitialSpeed) {
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-40_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, -8_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{-40_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state{0_m, -8_mps};
for (int i = 0; i < 900; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -223,14 +264,19 @@ TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
}
TEST(ExponentialProfileTest, TestHeuristic) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
std::vector<std::tuple<
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State, // initial
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State, // goal
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State> // inflection
// point
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::State, // initial
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::State, // goal
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::State> // inflection
// point
>
testCases{
// red > green and purple => always positive => false
@@ -275,13 +321,17 @@ TEST(ExponentialProfileTest, TestHeuristic) {
}
TEST(ExponentialProfileTest, TimingToCurrent) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{2_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{2_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state{0_m, 0_mps};
for (int i = 0; i < 900; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
@@ -292,13 +342,17 @@ TEST(ExponentialProfileTest, TimingToCurrent) {
}
TEST(ExponentialProfileTest, TimingToGoal) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{2_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{2_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state{0_m, 0_mps};
auto prediction = profile.TimeLeftUntil(state, goal);
auto reachedGoal = false;
@@ -315,13 +369,17 @@ TEST(ExponentialProfileTest, TimingToGoal) {
}
TEST(ExponentialProfileTest, TimingToNegativeGoal) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::Constraints constraints{
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{constraints};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts> profile{
constraints};
wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State goal{-2_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State state{0_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
goal{-2_m, 0_mps};
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state{0_m, 0_mps};
auto prediction = profile.TimeLeftUntil(state, goal);
auto reachedGoal = false;

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

@@ -26,8 +26,10 @@ TEST(RectangularRegionConstraintTest, Constraint) {
bool exceededConstraintOutsideRegion = false;
for (auto& point : trajectory.States()) {
if (rectangle.Contains(point.pose.Translation())) {
EXPECT_TRUE(wpi::units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
} else if (wpi::units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
EXPECT_TRUE(wpi::units::math::abs(point.velocity) <
maxVelocity + 0.05_mps);
} else if (wpi::units::math::abs(point.velocity) >=
maxVelocity + 0.05_mps) {
exceededConstraintOutsideRegion = true;
}
}

8
wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
View File

@@ -10,8 +10,9 @@
#include "wpi/math/trajectory/TrajectoryConfig.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
void TestSameShapedTrajectory(std::vector<wpi::math::Trajectory::State> statesA,
std::vector<wpi::math::Trajectory::State> statesB) {
void TestSameShapedTrajectory(
std::vector<wpi::math::Trajectory::State> statesA,
std::vector<wpi::math::Trajectory::State> statesB) {
for (unsigned int i = 0; i < statesA.size() - 1; i++) {
auto a1 = statesA[i].pose;
auto a2 = statesA[i + 1].pose;
@@ -48,7 +49,8 @@ TEST(TrajectoryTransformsTest, TransformBy) {
TEST(TrajectoryTransformsTest, RelativeTo) {
wpi::math::TrajectoryConfig config{3_mps, 3_mps_sq};
auto trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
wpi::math::Pose2d{1_m, 2_m, 30_deg}, {}, wpi::math::Pose2d{5_m, 7_m, 90_deg}, config);
wpi::math::Pose2d{1_m, 2_m, 30_deg}, {},
wpi::math::Pose2d{5_m, 7_m, 90_deg}, config);
auto transformedTrajectory = trajectory.RelativeTo({1_m, 2_m, 30_deg});

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

@@ -27,8 +27,8 @@ static constexpr auto kDt = 10_ms;
}
TEST(TrapezoidProfileTest, ReachesGoal) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{1.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
1.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{3_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
@@ -42,8 +42,8 @@ TEST(TrapezoidProfileTest, ReachesGoal) {
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
TEST(TrapezoidProfileTest, PosContinuousUnderVelChange) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{1.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
1.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{12_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
@@ -76,8 +76,8 @@ TEST(TrapezoidProfileTest, PosContinuousUnderVelChange) {
// There is some somewhat tricky code for dealing with going backwards
TEST(TrapezoidProfileTest, Backwards) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
@@ -89,8 +89,8 @@ TEST(TrapezoidProfileTest, Backwards) {
}
TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
@@ -110,8 +110,8 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
// Checks to make sure that it hits top speed
TEST(TrapezoidProfileTest, TopSpeed) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
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};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
@@ -129,8 +129,8 @@ TEST(TrapezoidProfileTest, TopSpeed) {
}
TEST(TrapezoidProfileTest, TimingToCurrent) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state;
@@ -144,13 +144,13 @@ TEST(TrapezoidProfileTest, TimingToCurrent) {
TEST(TrapezoidProfileTest, TimingToGoal) {
using wpi::units::unit_cast;
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto state = profile.Calculate(
kDt, goal, wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
@@ -168,13 +168,13 @@ TEST(TrapezoidProfileTest, TimingToGoal) {
TEST(TrapezoidProfileTest, TimingBeforeGoal) {
using wpi::units::unit_cast;
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto state = profile.Calculate(
kDt, goal, wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
bool reachedGoal = false;
@@ -191,13 +191,13 @@ TEST(TrapezoidProfileTest, TimingBeforeGoal) {
TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
using wpi::units::unit_cast;
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto state = profile.Calculate(
kDt, goal, wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
@@ -215,13 +215,13 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
using wpi::units::unit_cast;
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{-2_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto state = profile.Calculate(
kDt, goal, wpi::math::TrapezoidProfile<wpi::units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
bool reachedGoal = false;
@@ -236,15 +236,16 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
}
TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{1_mps, 1_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
1_mps, 1_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter> profile{constraints};
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
}
TEST(TrapezoidProfileTest, InitialVelocityConstraints) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{10_m, 0_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state{0_m, -10_mps};
@@ -258,8 +259,8 @@ TEST(TrapezoidProfileTest, InitialVelocityConstraints) {
}
TEST(TrapezoidProfileTest, GoalVelocityConstraints) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{10_m, 5_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state{0_m, 0.75_mps};
@@ -273,8 +274,8 @@ TEST(TrapezoidProfileTest, GoalVelocityConstraints) {
}
TEST(TrapezoidProfileTest, NegativeGoalVelocityConstraints) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{10_m, -5_mps};
wpi::math::TrapezoidProfile<wpi::units::meter>::State state{0_m, 0.75_mps};

24
wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp
View File

@@ -13,16 +13,20 @@ namespace {
using ProtoType = wpi::util::Protobuf<wpi::math::Trajectory>;
const Trajectory kExpectedData = Trajectory{std::vector<wpi::math::Trajectory::State>{
Trajectory::State{1.1_s, 2.2_mps, 3.3_mps_sq,
Pose2d(Translation2d(1.1_m, 2.2_m), Rotation2d(2.2_rad)),
wpi::units::curvature_t{6.6}},
Trajectory::State{2.1_s, 2.2_mps, 3.3_mps_sq,
Pose2d(Translation2d(2.1_m, 2.2_m), Rotation2d(2.2_rad)),
wpi::units::curvature_t{6.6}},
Trajectory::State{3.1_s, 2.2_mps, 3.3_mps_sq,
Pose2d(Translation2d(3.1_m, 2.2_m), Rotation2d(2.2_rad)),
wpi::units::curvature_t{6.6}}}};
const Trajectory kExpectedData =
Trajectory{std::vector<wpi::math::Trajectory::State>{
Trajectory::State{
1.1_s, 2.2_mps, 3.3_mps_sq,
Pose2d(Translation2d(1.1_m, 2.2_m), Rotation2d(2.2_rad)),
wpi::units::curvature_t{6.6}},
Trajectory::State{
2.1_s, 2.2_mps, 3.3_mps_sq,
Pose2d(Translation2d(2.1_m, 2.2_m), Rotation2d(2.2_rad)),
wpi::units::curvature_t{6.6}},
Trajectory::State{
3.1_s, 2.2_mps, 3.3_mps_sq,
Pose2d(Translation2d(3.1_m, 2.2_m), Rotation2d(2.2_rad)),
wpi::units::curvature_t{6.6}}}};
} // namespace
TEST(TrajectoryProtoTest, Roundtrip) {