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

[wpimath] Move math functionality into new wpimath library (#2629)

Browse Source 
The wpimath library is a new library designed to separate the reusable math functionality
from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j).

Package names / include file names were NOT changed to minimize breakage.  In a future year
it would be good to revamp these for a more uniform user experience and to reduce the risk
of accidental naming conflicts.

While theoretically all of this functionality could be placed into wpiutil, several pieces
of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this
expense for users who only want cscore or ntcore.  It also allows for easy future separation
of build tasks vs number of workers on memory-constrained machines.

This moves the following functionality from wpiutil into wpimath:
- Eigen
- ejml
- Drake
- DARE
- wpiutil.math package (Matrix etc)
- units

And the following functionality from wpilibc/j into wpimath:
- Geometry
- Kinematics
- Spline
- Trajectory
- LinearFilter
- MedianFilter
- Feed-forward controllers
This commit is contained in:
Peter Johnson
2020-08-06 23:57:39 -07:00
committed by GitHub
parent ad817d4f23
commit 42993b15c6
463 changed files with 1006 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
wpimath/src/test/native/cpp/EigenTest.cpp Normal file
View File

@@ -0,0 +1,61 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <Eigen/Core>
#include <Eigen/LU>
#include "gtest/gtest.h"
TEST(EigenTest, MultiplicationTest) {
Eigen::Matrix<double, 2, 2> m1;
m1 << 2, 1, 0, 1;
Eigen::Matrix<double, 2, 2> m2;
m2 << 3, 0, 0, 2.5;
const auto result = m1 * m2;
Eigen::Matrix<double, 2, 2> expectedResult;
expectedResult << 6.0, 2.5, 0.0, 2.5;
EXPECT_TRUE(expectedResult.isApprox(result));
Eigen::Matrix<double, 2, 3> m3;
m3 << 1.0, 3.0, 0.5, 2.0, 4.3, 1.2;
Eigen::Matrix<double, 3, 4> m4;
m4 << 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0;
const auto result2 = m3 * m4;
Eigen::Matrix<double, 2, 4> expectedResult2;
expectedResult2 << 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53;
EXPECT_TRUE(expectedResult2.isApprox(result2));
}
TEST(EigenTest, TransposeTest) {
Eigen::Matrix<double, 3, 1> vec;
vec << 1, 2, 3;
const auto transpose = vec.transpose();
Eigen::Matrix<double, 1, 3> expectedTranspose;
expectedTranspose << 1, 2, 3;
EXPECT_TRUE(expectedTranspose.isApprox(transpose));
}
TEST(EigenTest, InverseTest) {
Eigen::Matrix<double, 3, 3> mat;
mat << 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5;
const auto inverse = mat.inverse();
const auto identity = Eigen::MatrixXd::Identity(3, 3);
EXPECT_TRUE(identity.isApprox(mat * inverse));
}

94
wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp Normal file
View File

@@ -0,0 +1,94 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/LinearFilter.h" // NOLINT(build/include_order)
#include <cmath>
#include <memory>
#include <random>
#include <wpi/math>
#include "gtest/gtest.h"
#include "units/time.h"
// Filter constants
static constexpr units::second_t kFilterStep = 0.005_s;
static constexpr units::second_t kFilterTime = 2.0_s;
static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
static constexpr int32_t kMovAvgTaps = 6;
enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
std::ostream& operator<<(std::ostream& os,
const LinearFilterNoiseTestType& type) {
switch (type) {
case TEST_SINGLE_POLE_IIR:
os << "LinearFilter SinglePoleIIR";
break;
case TEST_MOVAVG:
os << "LinearFilter MovingAverage";
break;
}
return os;
}
static double GetData(double t) {
return 100.0 * std::sin(2.0 * wpi::math::pi * t);
}
class LinearFilterNoiseTest
: public testing::TestWithParam<LinearFilterNoiseTestType> {
protected:
std::unique_ptr<frc::LinearFilter<double>> m_filter;
void SetUp() override {
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
m_filter = std::make_unique<frc::LinearFilter<double>>(
frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
kFilterStep));
break;
}
case TEST_MOVAVG: {
m_filter = std::make_unique<frc::LinearFilter<double>>(
frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
break;
}
}
}
};
/**
* Test if the filter reduces the noise produced by a signal generator
*/
TEST_P(LinearFilterNoiseTest, NoiseReduce) {
double noiseGenError = 0.0;
double filterError = 0.0;
std::random_device rd;
std::mt19937 gen{rd()};
std::normal_distribution<double> distr{0.0, 10.0};
for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
double theory = GetData(t.to<double>());
double noise = distr(gen);
filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
noiseGenError += std::abs(noise - theory);
}
RecordProperty("FilterError", filterError);
// The filter should have produced values closer to the theory
EXPECT_GT(noiseGenError, filterError)
<< "Filter should have reduced noise accumulation but failed";
}
INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest,
testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));

136
wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp Normal file
View File

@@ -0,0 +1,136 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/LinearFilter.h" // NOLINT(build/include_order)
#include <cmath>
#include <functional>
#include <memory>
#include <random>
#include <wpi/math>
#include "gtest/gtest.h"
#include "units/time.h"
// Filter constants
static constexpr units::second_t kFilterStep = 0.005_s;
static constexpr units::second_t kFilterTime = 2.0_s;
static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
static constexpr double kHighPassTimeConstant = 0.006631;
static constexpr double kHighPassExpectedOutput = 10.074717;
static constexpr int32_t kMovAvgTaps = 6;
static constexpr double kMovAvgExpectedOutput = -10.191644;
enum LinearFilterOutputTestType {
TEST_SINGLE_POLE_IIR,
TEST_HIGH_PASS,
TEST_MOVAVG,
TEST_PULSE
};
std::ostream& operator<<(std::ostream& os,
const LinearFilterOutputTestType& type) {
switch (type) {
case TEST_SINGLE_POLE_IIR:
os << "LinearFilter SinglePoleIIR";
break;
case TEST_HIGH_PASS:
os << "LinearFilter HighPass";
break;
case TEST_MOVAVG:
os << "LinearFilter MovingAverage";
break;
case TEST_PULSE:
os << "LinearFilter Pulse";
break;
}
return os;
}
static double GetData(double t) {
return 100.0 * std::sin(2.0 * wpi::math::pi * t) +
20.0 * std::cos(50.0 * wpi::math::pi * t);
}
static double GetPulseData(double t) {
if (std::abs(t - 1.0) < 0.001) {
return 1.0;
} else {
return 0.0;
}
}
/**
* A fixture that includes a consistent data source wrapped in a filter
*/
class LinearFilterOutputTest
: public testing::TestWithParam<LinearFilterOutputTestType> {
protected:
std::unique_ptr<frc::LinearFilter<double>> m_filter;
std::function<double(double)> m_data;
double m_expectedOutput = 0.0;
void SetUp() override {
switch (GetParam()) {
case TEST_SINGLE_POLE_IIR: {
m_filter = std::make_unique<frc::LinearFilter<double>>(
frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
kFilterStep));
m_data = GetData;
m_expectedOutput = kSinglePoleIIRExpectedOutput;
break;
}
case TEST_HIGH_PASS: {
m_filter = std::make_unique<frc::LinearFilter<double>>(
frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
kFilterStep));
m_data = GetData;
m_expectedOutput = kHighPassExpectedOutput;
break;
}
case TEST_MOVAVG: {
m_filter = std::make_unique<frc::LinearFilter<double>>(
frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
m_data = GetData;
m_expectedOutput = kMovAvgExpectedOutput;
break;
}
case TEST_PULSE: {
m_filter = std::make_unique<frc::LinearFilter<double>>(
frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
m_data = GetPulseData;
m_expectedOutput = 0.0;
break;
}
}
}
};
/**
* Test if the linear filters produce consistent output for a given data set.
*/
TEST_P(LinearFilterOutputTest, Output) {
double filterOutput = 0.0;
for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
filterOutput = m_filter->Calculate(m_data(t.to<double>()));
}
RecordProperty("LinearFilterOutput", filterOutput);
EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
<< "Filter output didn't match expected value";
}
INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
TEST_MOVAVG, TEST_PULSE));

55
wpimath/src/test/native/cpp/MedianFilterTest.cpp Normal file
View File

@@ -0,0 +1,55 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/MedianFilter.h"
#include "gtest/gtest.h"
TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
frc::MedianFilter<double> filter{10};
filter.Calculate(3);
filter.Calculate(0);
filter.Calculate(4);
EXPECT_EQ(filter.Calculate(1000), 3.5);
}
TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
frc::MedianFilter<double> filter{10};
filter.Calculate(3);
filter.Calculate(0);
filter.Calculate(4);
filter.Calculate(7);
EXPECT_EQ(filter.Calculate(1000), 4);
}
TEST(MedianFilterTest, MedianFilterFullTestEven) {
frc::MedianFilter<double> filter{6};
filter.Calculate(3);
filter.Calculate(0);
filter.Calculate(0);
filter.Calculate(5);
filter.Calculate(4);
filter.Calculate(1000);
EXPECT_EQ(filter.Calculate(99), 4.5);
}
TEST(MedianFilterTest, MedianFilterFullTestOdd) {
frc::MedianFilter<double> filter{5};
filter.Calculate(3);
filter.Calculate(0);
filter.Calculate(5);
filter.Calculate(4);
filter.Calculate(1000);
EXPECT_EQ(filter.Calculate(99), 5);
}

3422
wpimath/src/test/native/cpp/UnitsTest.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

76
wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp Normal file
View File

@@ -0,0 +1,76 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <gtest/gtest.h>
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/math/autodiff.h"
using Eigen::MatrixXd;
namespace drake {
namespace math {
namespace {
void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
const Eigen::Ref<const MatrixXd>& B,
const Eigen::Ref<const MatrixXd>& Q,
const Eigen::Ref<const MatrixXd>& R) {
MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R);
// Check that X is positive semi-definite.
EXPECT_TRUE(
CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
int n = X.rows();
Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
for (int i = 0; i < n; i++) {
EXPECT_GE(es.eigenvalues()[i], 0);
}
// Check that X is the solution to the discrete time ARE.
MatrixXd Y = A.transpose() * X * A - X -
A.transpose() * X * B * (B.transpose() * X * B + R).inverse() *
B.transpose() * X * A +
Q;
EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
MatrixCompareType::absolute));
}
GTEST_TEST(DARE, SolveDAREandVerify) {
// Test 1: non-invertible A
// Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
// Riccati Equation"
int n1 = 4, m1 = 1;
MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1);
A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
B1 << 0, 0, 0, 1;
Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
R1 << 0.25;
SolveDAREandVerify(A1, B1, Q1, R1);
// Test 2: invertible A
int n2 = 2, m2 = 1;
MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2);
A2 << 1, 1, 0, 1;
B2 << 0, 1;
Q2 << 1, 0, 0, 0;
R2 << 0.3;
SolveDAREandVerify(A2, B2, Q2, R2);
// Test 3: the first generalized eigenvalue of (S,T) is stable
int n3 = 2, m3 = 1;
MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3);
A3 << 0, 1, 0, 0;
B3 << 0, 1;
Q3 << 1, 0, 0, 1;
R3 << 1;
SolveDAREandVerify(A3, B3, Q3, R3);
// Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix)
int n4 = 2, m4 = 2;
MatrixXd A4(n4, n4), B4(n4, m4), Q4(n4, n4), R4(m4, m4);
A4 << 1, 0, 0, 1;
B4 << 1, 0, 0, 1;
Q4 << 1, 0, 0, 1;
R4 << 1, 0, 0, 1;
SolveDAREandVerify(A4, B4, Q4, R4);
}
} // namespace
} // namespace math
} // namespace drake

62
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp Normal file
View File

@@ -0,0 +1,62 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <cmath>
#include "frc/geometry/Pose2d.h"
#include "gtest/gtest.h"
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Pose2dTest, TransformBy) {
const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
const auto transformed = initial + transform;
EXPECT_NEAR(transformed.X().to<double>(), 1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Y().to<double>(), 2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
}
TEST(Pose2dTest, RelativeTo) {
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
const auto finalRelativeToInitial = final.RelativeTo(initial);
EXPECT_NEAR(finalRelativeToInitial.X().to<double>(), 5.0 * std::sqrt(2.0),
kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
kEpsilon);
}
TEST(Pose2dTest, Equality) {
const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
EXPECT_TRUE(a == b);
}
TEST(Pose2dTest, Inequality) {
const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
EXPECT_TRUE(a != b);
}
TEST(Pose2dTest, Minus) {
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
const auto transform = final - initial;
EXPECT_NEAR(transform.X().to<double>(), 5.0 * std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transform.Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
}

67
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp Normal file
View File

@@ -0,0 +1,67 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <cmath>
#include <wpi/math>
#include "frc/geometry/Rotation2d.h"
#include "gtest/gtest.h"
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Rotation2dTest, RadiansToDegrees) {
const Rotation2d one{units::radian_t(wpi::math::pi / 3)};
const Rotation2d two{units::radian_t(wpi::math::pi / 4)};
EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
}
TEST(Rotation2dTest, DegreesToRadians) {
const auto one = Rotation2d(45.0_deg);
const auto two = Rotation2d(30.0_deg);
EXPECT_NEAR(one.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
}
TEST(Rotation2dTest, RotateByFromZero) {
const Rotation2d zero;
auto sum = zero + Rotation2d(90.0_deg);
EXPECT_NEAR(sum.Radians().to<double>(), wpi::math::pi / 2.0, kEpsilon);
EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
}
TEST(Rotation2dTest, RotateByNonZero) {
auto rot = Rotation2d(90.0_deg);
rot += Rotation2d(30.0_deg);
EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
}
TEST(Rotation2dTest, Minus) {
const auto one = Rotation2d(70.0_deg);
const auto two = Rotation2d(30.0_deg);
EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
}
TEST(Rotation2dTest, Equality) {
const auto one = Rotation2d(43_deg);
const auto two = Rotation2d(43_deg);
EXPECT_TRUE(one == two);
}
TEST(Rotation2dTest, Inequality) {
const auto one = Rotation2d(43_deg);
const auto two = Rotation2d(43.5_deg);
EXPECT_TRUE(one != two);
}

33
wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp Normal file
View File

@@ -0,0 +1,33 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <cmath>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Translation2d.h"
#include "gtest/gtest.h"
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Transform2dTest, Inverse) {
const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
auto transformed = initial + transform;
auto untransformed = transformed + transform.Inverse();
EXPECT_NEAR(initial.X().to<double>(), untransformed.X().to<double>(),
kEpsilon);
EXPECT_NEAR(initial.Y().to<double>(), untransformed.Y().to<double>(),
kEpsilon);
EXPECT_NEAR(initial.Rotation().Degrees().to<double>(),
untransformed.Rotation().Degrees().to<double>(), kEpsilon);
}

90
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp Normal file
View File

@@ -0,0 +1,90 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <cmath>
#include "frc/geometry/Translation2d.h"
#include "gtest/gtest.h"
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Translation2dTest, Sum) {
const Translation2d one{1.0_m, 3.0_m};
const Translation2d two{2.0_m, 5.0_m};
const auto sum = one + two;
EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
}
TEST(Translation2dTest, Difference) {
const Translation2d one{1.0_m, 3.0_m};
const Translation2d two{2.0_m, 5.0_m};
const auto difference = one - two;
EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
}
TEST(Translation2dTest, RotateBy) {
const Translation2d another{3.0_m, 0.0_m};
const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
}
TEST(Translation2dTest, Multiplication) {
const Translation2d original{3.0_m, 5.0_m};
const auto mult = original * 3;
EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
}
TEST(Translation2d, Division) {
const Translation2d original{3.0_m, 5.0_m};
const auto div = original / 2;
EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
}
TEST(Translation2dTest, Norm) {
const Translation2d one{3.0_m, 5.0_m};
EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
}
TEST(Translation2dTest, Distance) {
const Translation2d one{1_m, 1_m};
const Translation2d two{6_m, 6_m};
EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
}
TEST(Translation2dTest, UnaryMinus) {
const Translation2d original{-4.5_m, 7_m};
const auto inverted = -original;
EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
}
TEST(Translation2dTest, Equality) {
const Translation2d one{9_m, 5.5_m};
const Translation2d two{9_m, 5.5_m};
EXPECT_TRUE(one == two);
}
TEST(Translation2dTest, Inequality) {
const Translation2d one{9_m, 5.5_m};
const Translation2d two{9_m, 5.7_m};
EXPECT_TRUE(one != two);
}

69
wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp Normal file
View File

@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <cmath>
#include <wpi/math>
#include "frc/geometry/Pose2d.h"
#include "gtest/gtest.h"
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Twist2dTest, Straight) {
const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
const auto straightPose = Pose2d().Exp(straight);
EXPECT_NEAR(straightPose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
}
TEST(Twist2dTest, QuarterCircle) {
const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m,
units::radian_t(wpi::math::pi / 2.0)};
const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
EXPECT_NEAR(quarterCirclePose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
kEpsilon);
}
TEST(Twist2dTest, DiagonalNoDtheta) {
const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
const auto diagonalPose = Pose2d().Exp(diagonal);
EXPECT_NEAR(diagonalPose.X().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Y().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
}
TEST(Twist2dTest, Equality) {
const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
EXPECT_TRUE(one == two);
}
TEST(Twist2dTest, Inequality) {
const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
EXPECT_TRUE(one != two);
}
TEST(Twist2dTest, Pose2dLog) {
const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
const Pose2d start{};
const auto twist = start.Log(end);
EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
}

20
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp Normal file
View File

@@ -0,0 +1,20 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/ChassisSpeeds.h"
#include "gtest/gtest.h"
static constexpr double kEpsilon = 1E-9;
TEST(ChassisSpeeds, FieldRelativeConstruction) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
}

79
wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp Normal file
View File

@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <wpi/math>
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "gtest/gtest.h"
#include "units/angular_velocity.h"
#include "units/length.h"
#include "units/velocity.h"
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds;
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds;
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematics, 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);
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
}
TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps,
units::radians_per_second_t{wpi::math::pi}};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::math::pi, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::math::pi, kEpsilon);
}
TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{
units::meters_per_second_t(+0.381 * wpi::math::pi),
units::meters_per_second_t(-0.381 * wpi::math::pi)};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::math::pi, kEpsilon);
}

27
wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp Normal file
View File

@@ -0,0 +1,27 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <wpi/math>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/kinematics/DifferentialDriveOdometry.h"
#include "gtest/gtest.h"
static constexpr double kEpsilon = 1E-9;
using namespace frc;
TEST(DifferentialDriveOdometry, EncoderDistances) {
DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
units::meter_t(5 * wpi::math::pi));
EXPECT_NEAR(pose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
}

230
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp Normal file
View File

@@ -0,0 +1,230 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <wpi/math>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "gtest/gtest.h"
#include "units/angular_velocity.h"
using namespace frc;
class MecanumDriveKinematicsTest : public ::testing::Test {
protected:
Translation2d m_fl{12_m, 12_m};
Translation2d m_fr{12_m, -12_m};
Translation2d m_bl{-12_m, 12_m};
Translation2d m_br{-12_m, -12_m};
MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
};
TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
*/
EXPECT_NEAR(3.536, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(3.536, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(3.536, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(3.536, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
3.536_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
[[5][0][0]]
*/
EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
*/
EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(2.828427, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(2.828427, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(-2.828427, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
-2.828427_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
[[5][0][0]]
*/
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(4.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t(2 * wpi::math::pi)};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
*/
EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(106.62919, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(106.62919, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
-106.62919_mps, 106.62919_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should
be [[0][0][2pi]]
*/
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
*/
EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(20.506097, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-13.435, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(16.26, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps,
-13.435_mps, 16.26_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be
[[2][3][1]]
*/
EXPECT_NEAR(2.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(3.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
*/
EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(16.971, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-16.971, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(33.941, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps,
33.941_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from the
wheel velocities should be [[12][-12][1]]
*/
EXPECT_NEAR(12.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-12, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
OffCenterTranslationRotationInverseKinematics) {
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
*/
EXPECT_NEAR(2.12, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(21.92, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-12.02, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(36.06, moduleStates.rearRight.to<double>(), 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);
/*
By equation (13.13) of the state-space-guide, the chassis motion from the
wheel velocities should be [[17][-10][1]]
*/
EXPECT_NEAR(17.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-10, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
wheelSpeeds.Normalize(5.5_mps);
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
}

72
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp Normal file
View File

@@ -0,0 +1,72 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/MecanumDriveOdometry.h"
#include "gtest/gtest.h"
using namespace frc;
class MecanumDriveOdometryTest : public ::testing::Test {
protected:
Translation2d m_fl{12_m, 12_m};
Translation2d m_fr{12_m, -12_m};
Translation2d m_bl{-12_m, 12_m};
Translation2d m_br{-12_m, -12_m};
MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
MecanumDriveOdometry odometry{kinematics, 0_rad};
};
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
3.536_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
EXPECT_NEAR(secondPose.X().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
odometry.ResetPosition(Pose2d(), 0_rad);
MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
39.986_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.X().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
}

183
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp Normal file
View File

@@ -0,0 +1,183 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <wpi/math>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "gtest/gtest.h"
#include "units/angular_velocity.h"
using namespace frc;
static constexpr double kEpsilon = 0.1;
class SwerveDriveKinematicsTest : public ::testing::Test {
protected:
Translation2d m_fl{12_m, 12_m};
Translation2d m_fr{12_m, -12_m};
Translation2d m_bl{-12_m, 12_m};
Translation2d m_br{-12_m, -12_m};
SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
};
TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
SwerveModuleState state{5.0_mps, Rotation2d()};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
SwerveModuleState state{5_mps, Rotation2d(90_deg)};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t(2 * wpi::math::pi)};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t(2 * wpi::math::pi)};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
}
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));
EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
OffCenterCORRotationAndTranslationForwardKinematics) {
SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
SwerveModuleState state1{5.0_mps, Rotation2d()};
SwerveModuleState state2{6.0_mps, Rotation2d()};
SwerveModuleState state3{4.0_mps, Rotation2d()};
SwerveModuleState state4{7.0_mps, Rotation2d()};
std::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
}

74
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp Normal file
View File

@@ -0,0 +1,74 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/kinematics/SwerveDriveOdometry.h"
#include "gtest/gtest.h"
using namespace frc;
static constexpr double kEpsilon = 0.01;
class SwerveDriveOdometryTest : public ::testing::Test {
protected:
Translation2d m_fl{12_m, 12_m};
Translation2d m_fr{12_m, -12_m};
Translation2d m_bl{-12_m, 12_m};
Translation2d m_br{-12_m, -12_m};
SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
};
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
SwerveModuleState state{5_mps, Rotation2d()};
m_odometry.ResetPosition(Pose2d(), 0_rad);
m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
SwerveModuleState(), SwerveModuleState(),
SwerveModuleState());
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
state, state);
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
SwerveModuleState zero{0_mps, Rotation2d()};
m_odometry.ResetPosition(Pose2d(), 0_rad);
m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
auto pose =
m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
EXPECT_NEAR(12.0, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
SwerveModuleState state{5_mps, Rotation2d()};
m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
SwerveModuleState(), SwerveModuleState(),
SwerveModuleState());
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
state, state);
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
}

14
wpimath/src/test/native/cpp/main.cpp Normal file
View File

@@ -0,0 +1,14 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "gtest/gtest.h"
int main(int argc, char** argv) {
::testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}

128
wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp Normal file
View File

@@ -0,0 +1,128 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <chrono>
#include <iostream>
#include <vector>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/spline/QuinticHermiteSpline.h"
#include "frc/spline/SplineHelper.h"
#include "frc/spline/SplineParameterizer.h"
#include "gtest/gtest.h"
#include "units/length.h"
using namespace frc;
namespace frc {
class CubicHermiteSplineTest : public ::testing::Test {
protected:
static void Run(const Pose2d& a, const std::vector<Translation2d>& waypoints,
const Pose2d& b) {
// Start the timer.
const auto start = std::chrono::high_resolution_clock::now();
// Generate and parameterize the spline.
const auto [startCV, endCV] =
SplineHelper::CubicControlVectorsFromWaypoints(a, waypoints, b);
const auto splines =
SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV);
std::vector<Spline<3>::PoseWithCurvature> poses;
poses.push_back(splines[0].GetPoint(0.0));
for (auto&& spline : splines) {
auto x = SplineParameterizer::Parameterize(spline);
poses.insert(std::end(poses), std::begin(x) + 1, std::end(x));
}
// End timer.
const auto finish = std::chrono::high_resolution_clock::now();
// Calculate the duration (used when benchmarking)
const auto duration =
std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
for (unsigned int i = 0; i < poses.size() - 1; i++) {
auto& p0 = poses[i];
auto& p1 = poses[i + 1];
// Make sure the twist is under the tolerance defined by the Spline class.
auto twist = p0.first.Log(p1.first);
EXPECT_LT(std::abs(twist.dx.to<double>()),
SplineParameterizer::kMaxDx.to<double>());
EXPECT_LT(std::abs(twist.dy.to<double>()),
SplineParameterizer::kMaxDy.to<double>());
EXPECT_LT(std::abs(twist.dtheta.to<double>()),
SplineParameterizer::kMaxDtheta.to<double>());
}
// Check first point.
EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
a.Rotation().Radians().to<double>(), 1E-9);
// Check interior waypoints
bool interiorsGood = true;
for (auto& waypoint : waypoints) {
bool found = false;
for (auto& state : poses) {
if (std::abs(
waypoint.Distance(state.first.Translation()).to<double>()) <
1E-9) {
found = true;
}
}
interiorsGood &= found;
}
EXPECT_TRUE(interiorsGood);
// Check last point.
EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
b.Rotation().Radians().to<double>(), 1E-9);
static_cast<void>(duration);
}
};
} // namespace frc
TEST_F(CubicHermiteSplineTest, StraightLine) {
Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
}
TEST_F(CubicHermiteSplineTest, SCurve) {
Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
Translation2d(2_m, -1_m)};
Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
Run(start, waypoints, end);
}
TEST_F(CubicHermiteSplineTest, OneInterior) {
Pose2d start{0_m, 0_m, 0_rad};
std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
Pose2d end{4_m, 0_m, 0_rad};
Run(start, waypoints, end);
}
TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
EXPECT_THROW(
Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
SplineParameterizer::MalformedSplineException);
EXPECT_THROW(
Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
SplineParameterizer::MalformedSplineException);
}

92
wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp Normal file
View File

@@ -0,0 +1,92 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <chrono>
#include <iostream>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/spline/QuinticHermiteSpline.h"
#include "frc/spline/SplineHelper.h"
#include "frc/spline/SplineParameterizer.h"
#include "gtest/gtest.h"
#include "units/angle.h"
#include "units/length.h"
using namespace frc;
namespace frc {
class QuinticHermiteSplineTest : public ::testing::Test {
protected:
static void Run(const Pose2d& a, const Pose2d& b) {
// Start the timer.
const auto start = std::chrono::high_resolution_clock::now();
// Generate and parameterize the spline.
const auto spline = SplineHelper::QuinticSplinesFromControlVectors(
SplineHelper::QuinticControlVectorsFromWaypoints({a, b}))[0];
const auto poses = SplineParameterizer::Parameterize(spline);
// End timer.
const auto finish = std::chrono::high_resolution_clock::now();
// Calculate the duration (used when benchmarking)
const auto duration =
std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
for (unsigned int i = 0; i < poses.size() - 1; i++) {
auto& p0 = poses[i];
auto& p1 = poses[i + 1];
// Make sure the twist is under the tolerance defined by the Spline class.
auto twist = p0.first.Log(p1.first);
EXPECT_LT(std::abs(twist.dx.to<double>()),
SplineParameterizer::kMaxDx.to<double>());
EXPECT_LT(std::abs(twist.dy.to<double>()),
SplineParameterizer::kMaxDy.to<double>());
EXPECT_LT(std::abs(twist.dtheta.to<double>()),
SplineParameterizer::kMaxDtheta.to<double>());
}
// Check first point.
EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
a.Rotation().Radians().to<double>(), 1E-9);
// Check last point.
EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
b.Rotation().Radians().to<double>(), 1E-9);
static_cast<void>(duration);
}
};
} // namespace frc
TEST_F(QuinticHermiteSplineTest, StraightLine) {
Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
}
TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
}
TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
}
TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
Pose2d(1_m, 0_m, Rotation2d(180_deg))),
SplineParameterizer::MalformedSplineException);
EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
SplineParameterizer::MalformedSplineException);
}

45
wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp Normal file
View File

@@ -0,0 +1,45 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <memory>
#include <vector>
#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/acceleration.h"
#include "units/angle.h"
#include "units/math.h"
#include "units/velocity.h"
using namespace frc;
TEST(CentripetalAccelerationConstraintTest, Constraint) {
const auto maxCentripetalAcceleration = 7_fps_sq;
auto config = TrajectoryConfig(12_fps, 12_fps_sq);
config.AddConstraint(
CentripetalAccelerationConstraint(maxCentripetalAcceleration));
auto trajectory = TestTrajectory::GetTrajectory(config);
units::second_t time = 0_s;
units::second_t dt = 20_ms;
units::second_t duration = trajectory.TotalTime();
while (time < duration) {
const Trajectory::State point = trajectory.Sample(time);
time += dt;
auto centripetalAcceleration =
units::math::pow<2>(point.velocity) * point.curvature / 1_rad;
EXPECT_TRUE(centripetalAcceleration <
maxCentripetalAcceleration + 0.05_mps_sq);
}
}

45
wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp Normal file
View File

@@ -0,0 +1,45 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <memory>
#include <vector>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/time.h"
using namespace frc;
TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
const auto maxVelocity = 12_fps;
const DifferentialDriveKinematics kinematics{27_in};
auto config = TrajectoryConfig(12_fps, 12_fps_sq);
config.AddConstraint(
DifferentialDriveKinematicsConstraint(kinematics, maxVelocity));
auto trajectory = TestTrajectory::GetTrajectory(config);
units::second_t time = 0_s;
units::second_t dt = 20_ms;
units::second_t duration = trajectory.TotalTime();
while (time < duration) {
const Trajectory::State point = trajectory.Sample(time);
time += dt;
const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
point.velocity * point.curvature};
auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
EXPECT_TRUE(left < maxVelocity + 0.05_mps);
EXPECT_TRUE(right < maxVelocity + 0.05_mps);
}
}

87
wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp Normal file
View File

@@ -0,0 +1,87 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <iostream>
#include <memory>
#include <vector>
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/acceleration.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
#include "units/voltage.h"
using namespace frc;
TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
// Pick an unreasonably large kA to ensure the constraint has to do some work
SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
3_V / 1_mps_sq};
const DifferentialDriveKinematics kinematics{0.5_m};
const auto maxVoltage = 10_V;
auto config = TrajectoryConfig(12_fps, 12_fps_sq);
config.AddConstraint(
DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
auto trajectory = TestTrajectory::GetTrajectory(config);
units::second_t time = 0_s;
units::second_t dt = 20_ms;
units::second_t duration = trajectory.TotalTime();
while (time < duration) {
const Trajectory::State point = trajectory.Sample(time);
time += dt;
const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
point.velocity * point.curvature};
auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
// 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
// a reasonable check anyway
EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) <
maxVoltage + 0.05_V);
EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) >
-maxVoltage - 0.05_V);
EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) <
maxVoltage + 0.05_V);
EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) >
-maxVoltage - 0.05_V);
}
}
TEST(DifferentialDriveVoltageConstraintTest, HighCurvature) {
SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
3_V / 1_mps_sq};
// Large trackwidth - need to test with radius of curvature less than half of
// trackwidth
const DifferentialDriveKinematics kinematics{3_m};
const auto maxVoltage = 10_V;
auto config = TrajectoryConfig(12_fps, 12_fps_sq);
config.AddConstraint(
DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
Pose2d{1_m, 0_m, Rotation2d{90_deg}}, std::vector<Translation2d>{},
Pose2d{0_m, 1_m, Rotation2d{180_deg}}, config));
config.SetReversed(true);
EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
Pose2d{0_m, 1_m, Rotation2d{180_deg}}, std::vector<Translation2d>{},
Pose2d{1_m, 0_m, Rotation2d{90_deg}}, config));
}

61
wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp Normal file
View File

@@ -0,0 +1,61 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <vector>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/EllipticalRegionConstraint.h"
#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/acceleration.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/velocity.h"
using namespace frc;
TEST(EllipticalRegionConstraintTest, Constraint) {
constexpr auto maxVelocity = 2_fps;
auto config = TrajectoryConfig(13_fps, 13_fps_sq);
MaxVelocityConstraint maxVelConstraint(maxVelocity);
EllipticalRegionConstraint regionConstraint(frc::Translation2d{5_ft, 2.5_ft},
10_ft, 5_ft, Rotation2d(180_deg),
maxVelConstraint);
config.AddConstraint(regionConstraint);
auto trajectory = TestTrajectory::GetTrajectory(config);
bool exceededConstraintOutsideRegion = false;
for (auto& point : trajectory.States()) {
auto translation = point.pose.Translation();
if (translation.X() < 10_ft && translation.Y() < 5_ft) {
EXPECT_TRUE(units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
} else if (units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
exceededConstraintOutsideRegion = true;
}
}
EXPECT_TRUE(exceededConstraintOutsideRegion);
}
TEST(EllipticalRegionConstraintTest, IsPoseInRegion) {
constexpr auto maxVelocity = 2_fps;
MaxVelocityConstraint maxVelConstraint(maxVelocity);
EllipticalRegionConstraint regionConstraintNoRotation(
frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(0_deg),
maxVelConstraint);
EXPECT_FALSE(regionConstraintNoRotation.IsPoseInRegion(
frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
EllipticalRegionConstraint regionConstraintWithRotation(
frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(90_deg),
maxVelConstraint);
EXPECT_TRUE(regionConstraintWithRotation.IsPoseInRegion(
frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
}

56
wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp Normal file
View File

@@ -0,0 +1,56 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <vector>
#include "frc/kinematics/DifferentialDriveKinematics.h"
#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
#include "frc/trajectory/constraint/RectangularRegionConstraint.h"
#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/acceleration.h"
#include "units/length.h"
#include "units/math.h"
#include "units/velocity.h"
using namespace frc;
TEST(RectangularRegionConstraintTest, Constraint) {
constexpr auto maxVelocity = 2_fps;
auto config = TrajectoryConfig(13_fps, 13_fps_sq);
MaxVelocityConstraint maxVelConstraint(maxVelocity);
RectangularRegionConstraint regionConstraint(frc::Translation2d{1_ft, 1_ft},
frc::Translation2d{5_ft, 27_ft},
maxVelConstraint);
config.AddConstraint(regionConstraint);
auto trajectory = TestTrajectory::GetTrajectory(config);
bool exceededConstraintOutsideRegion = false;
for (auto& point : trajectory.States()) {
if (regionConstraint.IsPoseInRegion(point.pose)) {
EXPECT_TRUE(units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
} else if (units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
exceededConstraintOutsideRegion = true;
}
}
EXPECT_TRUE(exceededConstraintOutsideRegion);
}
TEST(RectangularRegionConstraintTest, IsPoseInRegion) {
constexpr auto maxVelocity = 2_fps;
MaxVelocityConstraint maxVelConstraint(maxVelocity);
RectangularRegionConstraint regionConstraint(frc::Translation2d{1_ft, 1_ft},
frc::Translation2d{5_ft, 27_ft},
maxVelConstraint);
EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d()));
EXPECT_TRUE(
regionConstraint.IsPoseInRegion(Pose2d(3_ft, 14.5_ft, Rotation2d())));
}

46
wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp Normal file
View File

@@ -0,0 +1,46 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <vector>
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
#include "units/math.h"
using namespace frc;
TEST(TrajectoryGenerationTest, ObeysConstraints) {
TrajectoryConfig config{12_fps, 12_fps_sq};
auto trajectory = TestTrajectory::GetTrajectory(config);
units::second_t time = 0_s;
units::second_t dt = 20_ms;
units::second_t duration = trajectory.TotalTime();
while (time < duration) {
const Trajectory::State point = trajectory.Sample(time);
time += dt;
EXPECT_TRUE(units::math::abs(point.velocity) <= 12_fps + 0.01_fps);
EXPECT_TRUE(units::math::abs(point.acceleration) <=
12_fps_sq + 0.01_fps_sq);
}
}
TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
const auto t = TrajectoryGenerator::GenerateTrajectory(
std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
Pose2d(1_m, 0_m, Rotation2d(180_deg))},
TrajectoryConfig(12_fps, 12_fps_sq));
ASSERT_EQ(t.States().size(), 1u);
ASSERT_EQ(t.TotalTime(), 0_s);
}

23
wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp Normal file
View File

@@ -0,0 +1,23 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/TrajectoryUtil.h"
#include "gtest/gtest.h"
#include "trajectory/TestTrajectory.h"
using namespace frc;
TEST(TrajectoryJsonTest, DeserializeMatches) {
TrajectoryConfig config{12_fps, 12_fps_sq};
auto trajectory = TestTrajectory::GetTrajectory(config);
Trajectory deserialized;
EXPECT_NO_THROW(deserialized = TrajectoryUtil::DeserializeTrajectory(
TrajectoryUtil::SerializeTrajectory(trajectory)));
EXPECT_EQ(trajectory.States(), deserialized.States());
}

68
wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp Normal file
View File

@@ -0,0 +1,68 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <vector>
#include "frc/trajectory/Trajectory.h"
#include "frc/trajectory/TrajectoryConfig.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"
void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
std::vector<frc::Trajectory::State> statesB) {
for (unsigned int i = 0; i < statesA.size() - 1; i++) {
auto a1 = statesA[i].pose;
auto a2 = statesA[i + 1].pose;
auto b1 = statesB[i].pose;
auto b2 = statesB[i + 1].pose;
auto a = a2.RelativeTo(a1);
auto b = b2.RelativeTo(b1);
EXPECT_NEAR(a.X().to<double>(), b.X().to<double>(), 1E-9);
EXPECT_NEAR(a.Y().to<double>(), b.Y().to<double>(), 1E-9);
EXPECT_NEAR(a.Rotation().Radians().to<double>(),
b.Rotation().Radians().to<double>(), 1E-9);
}
}
TEST(TrajectoryTransforms, TransformBy) {
frc::TrajectoryConfig config{3_mps, 3_mps_sq};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
config);
auto transformedTrajectory =
trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
auto firstPose = transformedTrajectory.Sample(0_s).pose;
EXPECT_NEAR(firstPose.X().to<double>(), 1.0, 1E-9);
EXPECT_NEAR(firstPose.Y().to<double>(), 2.0, 1E-9);
EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
}
TEST(TrajectoryTransforms, RelativeTo) {
frc::TrajectoryConfig config{3_mps, 3_mps_sq};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
auto transformedTrajectory =
trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
auto firstPose = transformedTrajectory.Sample(0_s).pose;
EXPECT_NEAR(firstPose.X().to<double>(), 0, 1E-9);
EXPECT_NEAR(firstPose.Y().to<double>(), 0, 1E-9);
EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
}

237
wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp Normal file
View File

@@ -0,0 +1,237 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/TrapezoidProfile.h" // NOLINT(build/include_order)
#include <chrono>
#include <cmath>
#include "gtest/gtest.h"
#include "units/acceleration.h"
#include "units/length.h"
#include "units/math.h"
#include "units/velocity.h"
static constexpr auto kDt = 10_ms;
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
EXPECT_LE(units::math::abs(val1 - val2), eps)
#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
if (val1 <= val2) { \
EXPECT_LE(val1, val2); \
} else { \
EXPECT_NEAR_UNITS(val1, val2, eps); \
}
TEST(TrapezoidProfileTest, ReachesGoal) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 450; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
}
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints.maxVelocity = 0.75_mps;
}
profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
state = profile.Calculate(kDt);
auto estimatedVel = (state.position - lastPos) / kDt;
if (i >= 400) {
// Since estimatedVel can have floating point rounding errors, we check
// whether value is less than or within an error delta of the new
// constraint.
EXPECT_LT_OR_NEAR_UNITS(estimatedVel, constraints.maxVelocity, 1e-4_mps);
EXPECT_LE(state.velocity, constraints.maxVelocity);
}
lastPos = state.position;
}
EXPECT_EQ(state, goal);
}
// There is some somewhat tricky code for dealing with going backwards
TEST(TrapezoidProfileTest, Backwards) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 400; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
}
TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 200; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_NE(state, goal);
goal = {0.0_m, 0.0_mps};
for (int i = 0; i < 550; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed
TEST(TrapezoidProfileTest, TopSpeed) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 200; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
for (int i = 0; i < 2000; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
}
TEST(TrapezoidProfileTest, TimingToCurrent) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 400; i++) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
}
}
TEST(TrapezoidProfileTest, TimingToGoal) {
using units::unit_cast;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
// time left in the profile doesn't increase linearly at the endpoints
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
reachedGoal = true;
}
}
}
TEST(TrapezoidProfileTest, TimingBeforeGoal) {
using units::unit_cast;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal &&
(units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
reachedGoal = true;
}
}
}
TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
using units::unit_cast;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
// time left in the profile doesn't increase linearly at the endpoints
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
reachedGoal = true;
}
}
}
TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
using units::unit_cast;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal &&
(units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
reachedGoal = true;
}
}
}