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

[sysid] Check data quality before OLS (#6110)

Browse Source
This commit is contained in:
Tyler Veness
2023-12-29 21:31:27 -08:00
committed by GitHub
parent 24a76be694
commit 47c5fd8620
7 changed files with 569 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
sysid/CMakeLists.txt
View File

@@ -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()

102
sysid/docs/arm-ols-with-angle-offset.md → sysid/docs/ols-derivations.md
View File

@@ -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

202
sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
View File

@@ -4,8 +4,13 @@
#include "sysid/analysis/FeedforwardAnalysis.h"
#include <array>
#include <bitset>
#include <cmath>
#include <Eigen/Eigenvalues>
#include <fmt/format.h>
#include <fmt/ranges.h>
#include <units/math.h>
#include <units/time.h>
@@ -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<PreparedData>& d,
const AnalysisType& type,
Eigen::Block<Eigen::MatrixXd> X,
Eigen::VectorBlock<Eigen::VectorXd> 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<Eigen::MatrixXd> 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<std::string_view> 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<double> 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<double> 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)

7
sysid/src/main/native/include/sysid/analysis/AnalysisManager.h
View File

@@ -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(); }
};
/**

32
sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h
View File

@@ -4,6 +4,7 @@
#pragma once
#include <string>
#include <tuple>
#include <vector>
@@ -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

14
sysid/src/main/native/include/sysid/analysis/FilteringUtils.h
View File

@@ -14,6 +14,7 @@
#include <utility>
#include <vector>
#include <fmt/format.h>
#include <frc/filter/LinearFilter.h>
#include <units/time.h>
#include <wpi/StringMap.h>
@@ -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 "

437
sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp
View File

@@ -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 <stdint.h>
#include <bitset>
#include <cmath>
#include <span>
#include <gtest/gtest.h>
#include <units/time.h>
#include <units/voltage.h>
#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 <typename Model>
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<const double> expectedGains,
std::span<const double> actualGains,
std::span<const double> 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<const double> expected,
std::span<const double> actual,
std::span<const double> 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 <typename Model>
void RunTests(Model& model, const sysid::AnalysisType& type,
std::span<const double> expectedGains,
std::span<const double> 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}});
}
}