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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-05-29 21:41:50 -07:00
parent 25eacfa226 ca05ffa1b9
commit 6c16e846fa
347 changed files with 18562 additions and 11557 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
wpimath/src/test/native/cpp/SleipnirTest.cpp
View File

@@ -3,24 +3,25 @@
// the WPILib BSD license file in the root directory of this project.
#include <gtest/gtest.h>
#include <sleipnir/optimization/OptimizationProblem.hpp>
#include <sleipnir/optimization/problem.hpp>
TEST(SleipnirTest, Quartic) {
sleipnir::OptimizationProblem problem;
slp::Problem problem;
auto x = problem.DecisionVariable();
x.SetValue(20.0);
auto x = problem.decision_variable();
x.set_value(20.0);
problem.Minimize(sleipnir::pow(x, 4));
problem.minimize(slp::pow(x, 4));
problem.SubjectTo(x >= 1);
problem.subject_to(x >= 1);
auto status = problem.Solve({.diagnostics = true});
EXPECT_EQ(problem.cost_function_type(), slp::ExpressionType::NONLINEAR);
EXPECT_EQ(problem.equality_constraint_type(), slp::ExpressionType::NONE);
EXPECT_EQ(problem.inequality_constraint_type(), slp::ExpressionType::LINEAR);
EXPECT_EQ(status.costFunctionType, sleipnir::ExpressionType::kNonlinear);
EXPECT_EQ(status.equalityConstraintType, sleipnir::ExpressionType::kNone);
EXPECT_EQ(status.inequalityConstraintType, sleipnir::ExpressionType::kLinear);
EXPECT_EQ(status.exitCondition, sleipnir::SolverExitCondition::kSuccess);
auto status = problem.solve({.diagnostics = true});
EXPECT_NEAR(x.Value(), 1.0, 1e-6);
EXPECT_EQ(status, slp::ExitStatus::SUCCESS);
EXPECT_NEAR(x.value(), 1.0, 1e-6);
}

31
wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
View File

@@ -33,6 +33,19 @@ TEST(StateSpaceUtilTest, CostArray) {
EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, CostDynamic) {
Eigen::MatrixXd mat = frc::MakeCostMatrix(std::vector{1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, CovParameterPack) {
constexpr frc::Matrixd<3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
@@ -59,6 +72,19 @@ TEST(StateSpaceUtilTest, CovArray) {
EXPECT_NEAR(mat(2, 2), 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, CovDynamic) {
Eigen::MatrixXd mat = frc::MakeCovMatrix(std::vector{1.0, 2.0, 3.0});
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 1), 4.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(2, 2), 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
[[maybe_unused]]
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
@@ -69,6 +95,11 @@ TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorDynamic) {
[[maybe_unused]]
Eigen::VectorXd vec = frc::MakeWhiteNoiseVector(std::vector{2.0, 3.0});
}
TEST(StateSpaceUtilTest, IsStabilizable) {
frc::Matrixd<2, 1> B{0, 1};

17
wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
View File

@@ -2,8 +2,6 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <cmath>
#include <gtest/gtest.h>
#include "frc/EigenCore.h"
@@ -22,10 +20,7 @@ Vectord<2> StateDynamics(const Vectord<2>& x) {
}
TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
std::function<Vectord<2>(const Vectord<2>&, const Vectord<1>&)>
modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics,
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{&Dynamics,
20_ms};
Vectord<2> r{2, 2};
@@ -35,14 +30,8 @@ TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
}
TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
std::function<Vectord<2>(const Vectord<2>&)> modelDynamics = [](auto& x) {
return StateDynamics(x);
};
Matrixd<2, 1> B{0, 1};
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics,
B, 20_ms};
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
&StateDynamics, Matrixd<2, 1>{{0.0}, {1.0}}, 20_ms};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};

31
wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
View File

@@ -22,6 +22,21 @@ TEST(AngleStatisticsTest, Mean) {
.isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3));
}
TEST(AngleStatisticsTest, Mean_DynamicSize) {
Eigen::MatrixXd sigmas{
{1, 1.2, 0},
{359 * std::numbers::pi / 180, 3 * std::numbers::pi / 180, 0},
{1, 2, 0}};
// Weights need to produce the mean of the sigmas
Eigen::VectorXd weights{3};
weights.fill(1.0 / sigmas.cols());
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(frc::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
sigmas, weights, 1),
1e-3));
}
TEST(AngleStatisticsTest, Residual) {
Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
@@ -30,9 +45,25 @@ TEST(AngleStatisticsTest, Residual) {
Eigen::Vector3d{0, 2 * std::numbers::pi / 180, 1}));
}
TEST(AngleStatisticsTest, Residual_DynamicSize) {
Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
EXPECT_TRUE(frc::AngleResidual<Eigen::Dynamic>(a, b, 1).isApprox(
Eigen::VectorXd{{0, 2 * std::numbers::pi / 180, 1}}));
}
TEST(AngleStatisticsTest, Add) {
Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
}
TEST(AngleStatisticsTest, Add_DynamicSize) {
Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
EXPECT_TRUE(frc::AngleAdd<Eigen::Dynamic>(a, b, 1).isApprox(
Eigen::VectorXd{{2, 0, 3}}));
}

36
wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
View File

@@ -28,6 +28,25 @@ TEST(NumericalJacobianTest, Bu) {
EXPECT_TRUE(newB.isApprox(B));
}
Eigen::VectorXd AxBuFn_DynamicSize(const Eigen::VectorXd& x,
const Eigen::VectorXd& u) {
return A * x + B * u;
}
// Test that we can recover A from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Ax_DynamicSize) {
Eigen::MatrixXd newA = frc::NumericalJacobianX(
AxBuFn_DynamicSize, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newA.isApprox(A));
}
// Test that we can recover B from AxBuFn() pretty accurately
TEST(NumericalJacobianTest, Bu_DynamicSize) {
Eigen::MatrixXd newB = frc::NumericalJacobianU(
AxBuFn_DynamicSize, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newB.isApprox(B));
}
frc::Matrixd<3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
frc::Matrixd<3, 2> D{{1, 1}, {2, 1}, {3, 2}};
@@ -49,3 +68,20 @@ TEST(NumericalJacobianTest, Du) {
CxDuFn, frc::Vectord<4>::Zero(), frc::Vectord<2>::Zero());
EXPECT_TRUE(newD.isApprox(D));
}
Eigen::VectorXd CxDuFn_DynamicSize(const Eigen::VectorXd& x,
const Eigen::VectorXd& u) {
return C * x + D * u;
}
TEST(NumericalJacobianTest, Cx_DynamicSize) {
Eigen::MatrixXd newC = frc::NumericalJacobianX(
CxDuFn_DynamicSize, Eigen::VectorXd::Zero(4), Eigen::VectorXd::Zero(2));
EXPECT_TRUE(newC.isApprox(C));
}
TEST(NumericalJacobianTest, Du_DynamicSize) {
Eigen::MatrixXd newD = frc::NumericalJacobianU(
CxDuFn_DynamicSize, Eigen::VectorXd::Zero(4), Eigen::VectorXd::Zero(2));
EXPECT_TRUE(newD.isApprox(D));
}