From 47c5fd862089a79dfb8cc30dacd0ab2a739c1b84 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 29 Dec 2023 21:31:27 -0800 Subject: [PATCH] [sysid] Check data quality before OLS (#6110) --- sysid/CMakeLists.txt | 6 + ...ith-angle-offset.md => ols-derivations.md} | 102 +++- .../cpp/analysis/FeedforwardAnalysis.cpp | 202 ++++++-- .../include/sysid/analysis/AnalysisManager.h | 7 +- .../sysid/analysis/FeedforwardAnalysis.h | 32 +- .../include/sysid/analysis/FilteringUtils.h | 14 +- .../cpp/analysis/FeedforwardAnalysisTest.cpp | 437 +++++++++++------- 7 files changed, 569 insertions(+), 231 deletions(-) rename sysid/docs/{arm-ols-with-angle-offset.md => ols-derivations.md} (64%) diff --git a/sysid/CMakeLists.txt b/sysid/CMakeLists.txt index a921402f4c..e8555e8e1b 100644 --- a/sysid/CMakeLists.txt +++ b/sysid/CMakeLists.txt @@ -19,6 +19,9 @@ elseif(APPLE) endif() add_executable(sysid ${sysid_src} ${sysid_resources_src} ${sysid_rc} ${APP_ICON_MACOSX}) +if(MSVC) + target_compile_options(sysid PRIVATE /utf-8) +endif() wpilib_link_macos_gui(sysid) wpilib_target_warnings(sysid) target_include_directories(sysid PRIVATE src/main/native/include) @@ -35,6 +38,9 @@ if(WITH_TESTS) wpilib_link_macos_gui(sysid_test) target_sources(sysid_test PRIVATE ${sysid_src}) target_compile_definitions(sysid_test PRIVATE RUNNING_SYSID_TESTS) + if(MSVC) + target_compile_options(sysid_test PRIVATE /utf-8) + endif() target_include_directories(sysid_test PRIVATE src/main/native/cpp src/main/native/include) target_link_libraries(sysid_test wpimath libglassnt libglass gtest) endif() diff --git a/sysid/docs/arm-ols-with-angle-offset.md b/sysid/docs/ols-derivations.md similarity index 64% rename from sysid/docs/arm-ols-with-angle-offset.md rename to sysid/docs/ols-derivations.md index ecb5c43982..53a8eb28fd 100644 --- a/sysid/docs/arm-ols-with-angle-offset.md +++ b/sysid/docs/ols-derivations.md @@ -1,28 +1,20 @@ -# Arm OLS with angle offset +# OLS derivations -If the arm encoder doesn't read zero degrees when the arm is horizontal, the fit -for `Kg` will be wrong. An angle offset should be added to the model like so. +## Simple/drivetrain + +Here's the ODE for a drivetrain. ``` -dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle + offset) -``` -Use a trig identity to split the cosine into two terms. -``` -dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka (cos(angle) cos(offset) - sin(angle) sin(offset)) -dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) cos(offset) + Kg/Ka sin(angle) sin(offset) -``` -Reorder multiplicands so the offset trig is absorbed by the OLS terms. -``` -dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(offset) cos(angle) + Kg/Ka sin(offset) sin(angle) +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) ``` -## OLS +### OLS setup -Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`. +Let `α = -Kv/Ka`, `β = 1/Ka`, and `γ = -Ks/Ka`. ``` -dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle) +dx/dt = αx + βu + γ sgn(x) ``` -### Ks, Kv, Ka +### Feedforward gains Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`. ``` @@ -31,7 +23,71 @@ Kv = -α/β Ka = 1/β ``` -### Kg +## Elevator + +Here's the ODE for an elevator. +``` +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka +``` + +### OLS setup + +Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, and `δ = -Kg/Ka`. +``` +dx/dt = αx + βu + γ sgn(x) + δ +``` + +### Feedforward gains + +Divide the OLS terms by each other to obtain `Ks`, `Kv`, `Ka`, and `Kg`. +``` +Ks = -γ/β +Kv = -α/β +Ka = 1/β +Kg = −δ/β +``` + +## Arm + +Here's the ODE for an arm: +``` +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) +``` + +If the arm encoder doesn't read zero degrees when the arm is horizontal, the fit +for `Kg` will be wrong. An angle offset should be added to the model like so. +``` +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle + offset) +``` + +Use a trig identity to split the cosine into two terms. +``` +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka (cos(angle) cos(offset) - sin(angle) sin(offset)) +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) cos(offset) + Kg/Ka sin(angle) sin(offset) +``` + +Reorder multiplicands so the offset trig is absorbed by the OLS terms. +``` +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(offset) cos(angle) + Kg/Ka sin(offset) sin(angle) +``` + +### OLS setup + +Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`. +``` +dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle) +``` + +### Feedforward gains: Ks, Kv, Ka + +Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`. +``` +Ks = -γ/β +Kv = -α/β +Ka = 1/β +``` + +### Feedforward gains: Kg Take the sum of squares of the OLS terms containing the angle offset. The angle offset trig functions will form a trig identity that cancels out. Then, just @@ -44,14 +100,12 @@ solve for `Kg`. δ²+ε² = (Kg/Ka)² (1) δ²+ε² = (Kg/Ka)² √(δ²+ε²) = Kg/Ka -√(δ²+ε²) = Kg β -Kg = √(δ²+ε²)/β +hypot(δ, ε) = Kg/Ka +hypot(δ, ε) = Kg β +Kg = hypot(δ, ε)/β ``` -As a sanity check, when the offset is zero, ε is zero and the equation for -`Kg` simplifies to -δ/β, the equation previously used by SysId. - -### Angle offset +### Feedforward gains: offset Divide ε by δ, combine the trig functions into `tan(offset)`, then use `atan2()` to preserve the angle quadrant. Maintaining the proper negative signs in the diff --git a/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp index 6bd5aa0745..a2c0f2d334 100644 --- a/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp @@ -4,8 +4,13 @@ #include "sysid/analysis/FeedforwardAnalysis.h" +#include +#include #include +#include +#include +#include #include #include @@ -16,7 +21,22 @@ namespace sysid { /** - * Populates OLS data for (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ). + * Populates OLS data for the following models: + * + * Simple, Drivetrain, DrivetrainAngular: + * + * (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ) + * + * Elevator: + * + * (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ) + δ + * + * Arm: + * + * (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ) + δ cos(angle) + ε sin(angle) + * + * OLS performs best with the noisiest variable as the dependent variable, so we + * regress acceleration in terms of the other variables. * * @param d List of characterization data. * @param type Type of system being identified. @@ -27,35 +47,123 @@ static void PopulateOLSData(const std::vector& d, const AnalysisType& type, Eigen::Block X, Eigen::VectorBlock y) { + // Fill in X and y row-wise for (size_t sample = 0; sample < d.size(); ++sample) { const auto& pt = d[sample]; - // Add the velocity term (for α) + // Set the velocity term (for α) X(sample, 0) = pt.velocity; - // Add the voltage term (for β) + // Set the voltage term (for β) X(sample, 1) = pt.voltage; - // Add the intercept term (for γ) + // Set the intercept term (for γ) X(sample, 2) = std::copysign(1, pt.velocity); - // Add test-specific variables + // Set test-specific variables if (type == analysis::kElevator) { - // Add the gravity term (for Kg) + // Set the gravity term (for δ) X(sample, 3) = 1.0; } else if (type == analysis::kArm) { - // Add the cosine and sine terms (for Kg) + // Set the cosine and sine terms (for δ and ε) X(sample, 3) = pt.cos; X(sample, 4) = pt.sin; } - // Add the dependent variable (acceleration) + // Set the dependent variable (acceleration) y(sample) = pt.acceleration; } } +/** + * Throws an InsufficientSamplesError if the collected data is poor for OLS. + * + * @param X The collected data in matrix form for OLS. + * @param type The analysis type. + */ +static void CheckOLSDataQuality(const Eigen::MatrixXd& X, + const AnalysisType& type) { + Eigen::SelfAdjointEigenSolver eigSolver{X.transpose() * X}; + const Eigen::VectorXd& eigvals = eigSolver.eigenvalues(); + const Eigen::MatrixXd& eigvecs = eigSolver.eigenvectors(); + + // Bits are Ks, Kv, Ka, Kg, offset + std::bitset<5> badGains; + + constexpr double threshold = 10.0; + + // For n x n matrix XᵀX, need n - 1 nonzero eigenvalues for good fit + for (int row = 0; row < eigvals.rows(); ++row) { + if (std::abs(eigvals(row)) <= threshold) { + // Find row of eigenvector with largest magnitude. This determines which + // gain is rank-deficient + int maxIndex; + eigvecs.col(row).cwiseAbs().maxCoeff(&maxIndex); + + // Fit for α is rank-deficient + if (maxIndex == 0) { + badGains.set(1); + } + // Fit for β is rank-deficient + if (maxIndex == 1) { + badGains.set(); + break; + } + // Fit for γ is rank-deficient + if (maxIndex == 2) { + badGains.set(0); + } + // Fit for δ is rank-deficient + if (maxIndex == 3) { + if (type == analysis::kElevator) { + badGains.set(3); + } else if (type == analysis::kArm) { + badGains.set(3); + badGains.set(4); + } + } + // Fit for ε is rank-deficient + if (maxIndex == 4) { + badGains.set(3); + badGains.set(4); + } + } + } + + // If any gains are bad, throw an error + if (badGains.any()) { + // Create list of bad gain names + constexpr std::array gainNames{"Ks", "Kv", "Ka", "Kg", "offset"}; + std::vector badGainsList; + for (size_t i = 0; i < badGains.size(); ++i) { + if (badGains.test(i)) { + badGainsList.emplace_back(gainNames[i]); + } + } + + std::string error = fmt::format("Insufficient samples to compute {}.\n\n", + fmt::join(badGainsList, ", ")); + + // If all gains are bad, the robot may not have moved + if (badGains.all()) { + error += "Either no data was collected or the robot didn't move.\n\n"; + } + + // Append guidance for fixing the data + error += + "Ensure the data has:\n\n" + " * at least 2 steady-state velocity events to separate Ks from Kv\n" + " * at least 1 acceleration event to find Ka\n" + " * for elevators, enough vertical motion to measure gravity\n" + " * for arms, enough range of motion to measure gravity and encoder " + "offset\n"; + throw InsufficientSamplesError{error}; + } +} + OLSResult CalculateFeedforwardGains(const Storage& data, - const AnalysisType& type) { + const AnalysisType& type, + bool throwOnBadData) { // Iterate through the data and add it to our raw vector. const auto& [slowForward, slowBackward, fastForward, fastBackward] = data; @@ -86,32 +194,64 @@ OLSResult CalculateFeedforwardGains(const Storage& data, X.block(rowOffset, 0, fastBackward.size(), X.cols()), y.segment(rowOffset, fastBackward.size())); - // Perform OLS with accel = alpha*vel + beta*voltage + gamma*signum(vel) - // OLS performs best with the noisiest variable as the dependent var, - // so we regress accel in terms of the other variables. - auto ols = OLS(X, y); - double alpha = ols.coeffs[0]; // -Kv/Ka - double beta = ols.coeffs[1]; // 1/Ka - double gamma = ols.coeffs[2]; // -Ks/Ka - - // Initialize gains list with Ks, Kv, and Ka - std::vector gains{-gamma / beta, -alpha / beta, 1 / beta}; - - if (type == analysis::kElevator) { - // Add Kg to gains list - double delta = ols.coeffs[3]; // -Kg/Ka - gains.emplace_back(-delta / beta); + // Check quality of collected data + if (throwOnBadData) { + CheckOLSDataQuality(X, type); } - if (type == analysis::kArm) { - double delta = ols.coeffs[3]; // -Kg/Ka cos(offset) - double epsilon = ols.coeffs[4]; // Kg/Ka sin(offset) + std::vector gains; + gains.reserve(X.rows()); - // Add Kg to gains list - gains.emplace_back(std::hypot(delta, epsilon) / beta); + auto ols = OLS(X, y); - // Add offset to gains list - gains.emplace_back(std::atan2(epsilon, -delta)); + // Calculate feedforward gains + // + // See docs/ols-derivations.md for more details. + { + // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) + // dx/dt = αx + βu + γ sgn(x) + + // α = -Kv/Ka + // β = 1/Ka + // γ = -Ks/Ka + double α = ols.coeffs[0]; + double β = ols.coeffs[1]; + double γ = ols.coeffs[2]; + + // Ks = -γ/β + // Kv = -α/β + // Ka = 1/β + gains.emplace_back(-γ / β); + gains.emplace_back(-α / β); + gains.emplace_back(1 / β); + + if (type == analysis::kElevator) { + // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka + // dx/dt = αx + βu + γ sgn(x) + δ + + // δ = -Kg/Ka + double δ = ols.coeffs[3]; + + // Kg = -δ/β + gains.emplace_back(-δ / β); + } + + if (type == analysis::kArm) { + // dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) + // - Kg/Ka cos(offset) cos(angle) NOLINT + // + Kg/Ka sin(offset) sin(angle) NOLINT + // dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle) NOLINT + + // δ = -Kg/Ka cos(offset) + // ε = Kg/Ka sin(offset) + double δ = ols.coeffs[3]; + double ε = ols.coeffs[4]; + + // Kg = hypot(δ, ε)/β NOLINT + // offset = atan2(ε, -δ) NOLINT + gains.emplace_back(std::hypot(δ, ε) / β); + gains.emplace_back(std::atan2(ε, -δ)); + } } // Gains are Ks, Kv, Ka, Kg (elevator/arm only), offset (arm only) diff --git a/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h index 4c081f27ae..956b3b58be 100644 --- a/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h +++ b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h @@ -109,7 +109,8 @@ class AnalysisManager { /** * Exception for File Reading Errors. */ - struct FileReadingError : public std::exception { + class FileReadingError : public std::exception { + public: /** * Creates a FileReadingError object * @@ -119,11 +120,13 @@ class AnalysisManager { msg = fmt::format("Unable to read: {}", path); } + const char* what() const noexcept override { return msg.c_str(); } + + private: /** * The path of the file that was opened. */ std::string msg; - const char* what() const noexcept override { return msg.c_str(); } }; /** diff --git a/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h b/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h index bd3b83645b..d2503ce5cc 100644 --- a/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h +++ b/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include @@ -13,11 +14,40 @@ namespace sysid { +/** + * Exception for data that doesn't sample enough of the state-input space. + */ +class InsufficientSamplesError : public std::exception { + public: + /** + * Constructs an InsufficientSamplesError. + * + * @param message The error message + */ + explicit InsufficientSamplesError(std::string_view message) { + m_message = message; + } + + const char* what() const noexcept override { return m_message.c_str(); } + + private: + /** + * Stores the error message + */ + std::string m_message; +}; + /** * Calculates feedforward gains given the data and the type of analysis to * perform. + * + * @param data The OLS input data. + * @param type The analysis type. + * @param throwOnRankDeficiency Whether to throw if the fit is going to be poor. + * This option is provided for unit testing purposes. */ OLSResult CalculateFeedforwardGains(const Storage& data, - const AnalysisType& type); + const AnalysisType& type, + bool throwOnRankDeficiency = true); } // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h index 9030c00753..9ea993da1d 100644 --- a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h +++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -30,7 +31,8 @@ constexpr int kNoiseMeanWindow = 9; * Exception for Invalid Data Errors in which we can't pin the cause of error to * any one specific setting of the GUI. */ -struct InvalidDataError : public std::exception { +class InvalidDataError : public std::exception { + public: /** * Creates an InvalidDataError Exception. It adds additional steps after the * initial error message to inform users in the ways that they could fix their @@ -46,17 +48,20 @@ struct InvalidDataError : public std::exception { message); } + const char* what() const noexcept override { return m_message.c_str(); } + + private: /** * Stores the error message */ std::string m_message; - const char* what() const noexcept override { return m_message.c_str(); } }; /** * Exception for Quasistatic Data being completely removed. */ -struct NoQuasistaticDataError : public std::exception { +class NoQuasistaticDataError : public std::exception { + public: const char* what() const noexcept override { return "Quasistatic test trimming removed all data. Please adjust your " "motion threshold and double check " @@ -68,7 +73,8 @@ struct NoQuasistaticDataError : public std::exception { /** * Exception for Dynamic Data being completely removed. */ -struct NoDynamicDataError : public std::exception { +class NoDynamicDataError : public std::exception { + public: const char* what() const noexcept override { return "Dynamic test trimming removed all data. Please adjust your test " "duration and double check " diff --git a/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp index d5ff9e28bb..7a087d85b7 100644 --- a/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp +++ b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp @@ -2,28 +2,43 @@ // 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 + +#include #include +#include #include #include #include #include "sysid/analysis/AnalysisManager.h" +#include "sysid/analysis/AnalysisType.h" #include "sysid/analysis/ArmSim.h" #include "sysid/analysis/ElevatorSim.h" #include "sysid/analysis/FeedforwardAnalysis.h" #include "sysid/analysis/SimpleMotorSim.h" +namespace { + +enum Movements : uint32_t { + kSlowForward, + kSlowBackward, + kFastForward, + kFastBackward +}; + +constexpr int kMovementCombinations = 16; + /** * Return simulated test data for a given simulation model. * - * @param Ks Static friction gain. - * @param Kv Velocity gain. - * @param Ka Acceleration gain. - * @param Kg Gravity cosine gain. + * @tparam Model The model type. + * @param model The simulation model. + * @param movements Which movements to do. */ template -sysid::Storage CollectData(Model& model) { +sysid::Storage CollectData(Model& model, std::bitset<4> movements) { constexpr auto kUstep = 0.25_V / 1_s; constexpr units::volt_t kUmax = 7_V; constexpr units::second_t T = 5_ms; @@ -31,211 +46,295 @@ sysid::Storage CollectData(Model& model) { sysid::Storage storage; auto& [slowForward, slowBackward, fastForward, fastBackward] = storage; - - // Slow forward test auto voltage = 0_V; - for (int i = 0; i < (kTestDuration / T).value(); ++i) { - slowForward.emplace_back(sysid::PreparedData{ - i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, - model.GetAcceleration(voltage), std::cos(model.GetPosition()), - std::sin(model.GetPosition())}); - model.Update(voltage, T); - voltage += kUstep * T; + // Slow forward + if (movements.test(Movements::kSlowForward)) { + model.Reset(); + voltage = 0_V; + for (int i = 0; i < (kTestDuration / T).value(); ++i) { + slowForward.emplace_back(sysid::PreparedData{ + i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, + model.GetAcceleration(voltage), std::cos(model.GetPosition()), + std::sin(model.GetPosition())}); + + model.Update(voltage, T); + voltage += kUstep * T; + } } - // Slow backward test - model.Reset(); - voltage = 0_V; - for (int i = 0; i < (kTestDuration / T).value(); ++i) { - slowBackward.emplace_back(sysid::PreparedData{ - i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, - model.GetAcceleration(voltage), std::cos(model.GetPosition()), - std::sin(model.GetPosition())}); + // Slow backward + if (movements.test(Movements::kSlowBackward)) { + model.Reset(); + voltage = 0_V; + for (int i = 0; i < (kTestDuration / T).value(); ++i) { + slowBackward.emplace_back(sysid::PreparedData{ + i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, + model.GetAcceleration(voltage), std::cos(model.GetPosition()), + std::sin(model.GetPosition())}); - model.Update(voltage, T); - voltage -= kUstep * T; + model.Update(voltage, T); + voltage -= kUstep * T; + } } - // Fast forward test - model.Reset(); - voltage = 0_V; - for (int i = 0; i < (kTestDuration / T).value(); ++i) { - fastForward.emplace_back(sysid::PreparedData{ - i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, - model.GetAcceleration(voltage), std::cos(model.GetPosition()), - std::sin(model.GetPosition())}); + // Fast forward + if (movements.test(Movements::kFastForward)) { + model.Reset(); + voltage = 0_V; + for (int i = 0; i < (kTestDuration / T).value(); ++i) { + fastForward.emplace_back(sysid::PreparedData{ + i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, + model.GetAcceleration(voltage), std::cos(model.GetPosition()), + std::sin(model.GetPosition())}); - model.Update(voltage, T); - voltage = kUmax; + model.Update(voltage, T); + voltage = kUmax; + } } - // Fast backward test - model.Reset(); - voltage = 0_V; - for (int i = 0; i < (kTestDuration / T).value(); ++i) { - fastBackward.emplace_back(sysid::PreparedData{ - i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, - model.GetAcceleration(voltage), std::cos(model.GetPosition()), - std::sin(model.GetPosition())}); + // Fast backward + if (movements.test(Movements::kFastBackward)) { + model.Reset(); + voltage = 0_V; + for (int i = 0; i < (kTestDuration / T).value(); ++i) { + fastBackward.emplace_back(sysid::PreparedData{ + i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, + model.GetAcceleration(voltage), std::cos(model.GetPosition()), + std::sin(model.GetPosition())}); - model.Update(voltage, T); - voltage = -kUmax; + model.Update(voltage, T); + voltage = -kUmax; + } } return storage; } -TEST(FeedforwardAnalysisTest, Arm1) { - constexpr double Ks = 1.01; - constexpr double Kv = 3.060; - constexpr double Ka = 0.327; - constexpr double Kg = 0.211; +/** + * Asserts success if the gains contain NaNs or are too far from their expected + * values. + * + * @param expectedGains The expected feedforward gains. + * @param actualGains The calculated feedforward gains. + * @param tolerances The tolerances for the coefficient comparisons. + */ +testing::AssertionResult FitIsBad(std::span expectedGains, + std::span actualGains, + std::span tolerances) { + // Check for NaN + for (const auto& coeff : actualGains) { + if (std::isnan(coeff)) { + return testing::AssertionSuccess(); + } + } - for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) { - sysid::ArmSim model{Ks, Kv, Ka, Kg, offset}; - auto ff = sysid::CalculateFeedforwardGains(CollectData(model), - sysid::analysis::kArm); + for (size_t i = 0; i < expectedGains.size(); ++i) { + if (std::abs(expectedGains[i] - actualGains[i]) >= tolerances[i]) { + return testing::AssertionSuccess(); + } + } - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); - EXPECT_NEAR(ff.coeffs[3], Kg, 0.003); - EXPECT_NEAR(ff.coeffs[4], offset, 0.007); + auto result = testing::AssertionFailure(); + + result << "\n"; + for (size_t i = 0; i < expectedGains.size(); ++i) { + if (i == 0) { + result << "Ks"; + } else if (i == 1) { + result << "Kv"; + } else if (i == 2) { + result << "Ka"; + } else if (i == 3) { + result << "Kg"; + } else if (i == 4) { + result << "offset"; + } + + result << ":\n"; + result << " expected " << expectedGains[i] << ",\n"; + result << " actual " << actualGains[i] << ",\n"; + result << " diff " << std::abs(expectedGains[i] - actualGains[i]) << "\n"; + } + + return result; +} + +/** + * Asserts that two arrays are equal. + * + * @param expected The expected array. + * @param actual The actual array. + * @param tolerances The tolerances for the element comparisons. + */ +void ExpectArrayNear(std::span expected, + std::span actual, + std::span tolerances) { + // Check size + const size_t size = expected.size(); + EXPECT_EQ(size, actual.size()); + EXPECT_EQ(size, tolerances.size()); + + // Check elements + for (size_t i = 0; i < size; ++i) { + EXPECT_NEAR(expected[i], actual[i], tolerances[i]) << "where i = " << i; } } -TEST(FeedforwardAnalysisTest, Arm2) { - constexpr double Ks = 0.547; - constexpr double Kv = 0.0693; - constexpr double Ka = 0.1170; - constexpr double Kg = 0.122; +/** + * @tparam Model The model type. + * @param model The simulation model. + * @param type The analysis type. + * @param expectedGains The expected feedforward gains. + * @param tolerances The tolerances for the coefficient comparisons. + */ +template +void RunTests(Model& model, const sysid::AnalysisType& type, + std::span expectedGains, + std::span tolerances) { + // Iterate through all combinations of movements + for (int movements = 0; movements < kMovementCombinations; ++movements) { + try { + auto ff = + sysid::CalculateFeedforwardGains(CollectData(model, movements), type); - for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) { - sysid::ArmSim model{Ks, Kv, Ka, Kg, offset}; - auto ff = sysid::CalculateFeedforwardGains(CollectData(model), - sysid::analysis::kArm); - - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); - EXPECT_NEAR(ff.coeffs[3], Kg, 0.003); - EXPECT_NEAR(ff.coeffs[4], offset, 0.007); + ExpectArrayNear(expectedGains, ff.coeffs, tolerances); + } catch (sysid::InsufficientSamplesError&) { + // If calculation threw an exception, confirm at least one of the gains + // doesn't match + auto ff = sysid::CalculateFeedforwardGains(CollectData(model, movements), + type, false); + EXPECT_TRUE(FitIsBad(expectedGains, ff.coeffs, tolerances)); + } } } -TEST(FeedforwardAnalysisTest, Drivetrain1) { - constexpr double Ks = 1.01; - constexpr double Kv = 3.060; - constexpr double Ka = 0.327; +} // namespace - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - auto ff = sysid::CalculateFeedforwardGains(CollectData(model), - sysid::analysis::kDrivetrain); +TEST(FeedforwardAnalysisTest, Arm) { + { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; + constexpr double Kg = 0.211; - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); + for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) { + sysid::ArmSim model{Ks, Kv, Ka, Kg, offset}; + + RunTests(model, sysid::analysis::kArm, {{Ks, Kv, Ka, Kg, offset}}, + {{8e-3, 8e-3, 8e-3, 8e-3, 3e-2}}); + } + } + + { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + constexpr double Kg = 0.122; + + for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) { + sysid::ArmSim model{Ks, Kv, Ka, Kg, offset}; + + RunTests(model, sysid::analysis::kArm, {{Ks, Kv, Ka, Kg, offset}}, + {{8e-3, 8e-3, 8e-3, 8e-3, 5e-2}}); + } + } } -TEST(FeedforwardAnalysisTest, Drivetrain2) { - constexpr double Ks = 0.547; - constexpr double Kv = 0.0693; - constexpr double Ka = 0.1170; +TEST(FeedforwardAnalysisTest, Drivetrain) { + { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - auto ff = sysid::CalculateFeedforwardGains(CollectData(model), - sysid::analysis::kDrivetrain); + sysid::SimpleMotorSim model{Ks, Kv, Ka}; - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); + RunTests(model, sysid::analysis::kDrivetrain, {{Ks, Kv, Ka}}, + {{8e-3, 8e-3, 8e-3}}); + } + + { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + + sysid::SimpleMotorSim model{Ks, Kv, Ka}; + + RunTests(model, sysid::analysis::kDrivetrain, {{Ks, Kv, Ka}}, + {{8e-3, 8e-3, 8e-3}}); + } } -TEST(FeedforwardAnalysisTest, DrivetrainAngular1) { - constexpr double Ks = 1.01; - constexpr double Kv = 3.060; - constexpr double Ka = 0.327; +TEST(FeedforwardAnalysisTest, DrivetrainAngular) { + { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - auto ff = sysid::CalculateFeedforwardGains( - CollectData(model), sysid::analysis::kDrivetrainAngular); + sysid::SimpleMotorSim model{Ks, Kv, Ka}; - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); + RunTests(model, sysid::analysis::kDrivetrainAngular, {{Ks, Kv, Ka}}, + {{8e-3, 8e-3, 8e-3}}); + } + + { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + + sysid::SimpleMotorSim model{Ks, Kv, Ka}; + + RunTests(model, sysid::analysis::kDrivetrainAngular, {{Ks, Kv, Ka}}, + {{8e-3, 8e-3, 8e-3}}); + } } -TEST(FeedforwardAnalysisTest, DrivetrainAngular2) { - constexpr double Ks = 0.547; - constexpr double Kv = 0.0693; - constexpr double Ka = 0.1170; +TEST(FeedforwardAnalysisTest, Elevator) { + { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; + constexpr double Kg = -0.211; - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - auto ff = sysid::CalculateFeedforwardGains( - CollectData(model), sysid::analysis::kDrivetrainAngular); + sysid::ElevatorSim model{Ks, Kv, Ka, Kg}; - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); + RunTests(model, sysid::analysis::kElevator, {{Ks, Kv, Ka, Kg}}, + {{8e-3, 8e-3, 8e-3, 8e-3}}); + } + + { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + constexpr double Kg = -0.122; + + sysid::ElevatorSim model{Ks, Kv, Ka, Kg}; + + RunTests(model, sysid::analysis::kElevator, {{Ks, Kv, Ka, Kg}}, + {{8e-3, 8e-3, 8e-3, 8e-3}}); + } } -TEST(FeedforwardAnalysisTest, Elevator1) { - constexpr double Ks = 1.01; - constexpr double Kv = 3.060; - constexpr double Ka = 0.327; - constexpr double Kg = -0.211; +TEST(FeedforwardAnalysisTest, Simple) { + { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; - sysid::ElevatorSim model{Ks, Kv, Ka, Kg}; - auto ff = sysid::CalculateFeedforwardGains(CollectData(model), - sysid::analysis::kElevator); + sysid::SimpleMotorSim model{Ks, Kv, Ka}; - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); - EXPECT_NEAR(ff.coeffs[3], Kg, 0.003); -} - -TEST(FeedforwardAnalysisTest, Elevator2) { - constexpr double Ks = 0.547; - constexpr double Kv = 0.0693; - constexpr double Ka = 0.1170; - constexpr double Kg = -0.122; - - sysid::ElevatorSim model{Ks, Kv, Ka, Kg}; - auto ff = sysid::CalculateFeedforwardGains(CollectData(model), - sysid::analysis::kElevator); - - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); - EXPECT_NEAR(ff.coeffs[3], Kg, 0.003); -} - -TEST(FeedforwardAnalysisTest, Simple1) { - constexpr double Ks = 1.01; - constexpr double Kv = 3.060; - constexpr double Ka = 0.327; - - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - auto ff = sysid::CalculateFeedforwardGains(CollectData(model), - sysid::analysis::kSimple); - - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); -} - -TEST(FeedforwardAnalysisTest, Simple2) { - constexpr double Ks = 0.547; - constexpr double Kv = 0.0693; - constexpr double Ka = 0.1170; - - sysid::SimpleMotorSim model{Ks, Kv, Ka}; - auto ff = sysid::CalculateFeedforwardGains(CollectData(model), - sysid::analysis::kSimple); - - EXPECT_NEAR(ff.coeffs[0], Ks, 0.003); - EXPECT_NEAR(ff.coeffs[1], Kv, 0.003); - EXPECT_NEAR(ff.coeffs[2], Ka, 0.003); + RunTests(model, sysid::analysis::kSimple, {{Ks, Kv, Ka}}, + {{8e-3, 8e-3, 8e-3}}); + } + + { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + + sysid::SimpleMotorSim model{Ks, Kv, Ka}; + + RunTests(model, sysid::analysis::kSimple, {{Ks, Kv, Ka}}, + {{8e-3, 8e-3, 8e-3}}); + } }