mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
SCRIPT Move subprojects
This commit is contained in:
PJ Reiniger
committed by
Peter Johnson
Peter Johnson
parent
8cfc158790
commit
a5492d30da
213
tools/sysid/src/main/native/cpp/App.cpp
Normal file
213
tools/sysid/src/main/native/cpp/App.cpp
Normal file
@@ -0,0 +1,213 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 <cstdio>
|
||||
|
||||
#ifndef RUNNING_SYSID_TESTS
|
||||
|
||||
#include <filesystem>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <string_view>
|
||||
|
||||
#include <glass/Context.h>
|
||||
#include <glass/MainMenuBar.h>
|
||||
#include <glass/Storage.h>
|
||||
#include <glass/Window.h>
|
||||
#include <glass/WindowManager.h>
|
||||
#include <glass/other/Log.h>
|
||||
#include <imgui.h>
|
||||
#include <wpi/Logger.h>
|
||||
#include <wpi/print.h>
|
||||
#include <wpigui.h>
|
||||
#include <wpigui_openurl.h>
|
||||
|
||||
#include "sysid/view/Analyzer.h"
|
||||
#include "sysid/view/DataSelector.h"
|
||||
#include "sysid/view/LogLoader.h"
|
||||
#include "sysid/view/UILayout.h"
|
||||
|
||||
namespace gui = wpi::gui;
|
||||
|
||||
static std::unique_ptr<glass::WindowManager> gWindowManager;
|
||||
|
||||
glass::Window* gLogLoaderWindow;
|
||||
glass::Window* gDataSelectorWindow;
|
||||
glass::Window* gAnalyzerWindow;
|
||||
glass::Window* gProgramLogWindow;
|
||||
static glass::MainMenuBar gMainMenu;
|
||||
|
||||
glass::LogData gLog;
|
||||
wpi::Logger gLogger;
|
||||
|
||||
const char* GetWPILibVersion();
|
||||
|
||||
namespace sysid {
|
||||
std::string_view GetResource_sysid_16_png();
|
||||
std::string_view GetResource_sysid_32_png();
|
||||
std::string_view GetResource_sysid_48_png();
|
||||
std::string_view GetResource_sysid_64_png();
|
||||
std::string_view GetResource_sysid_128_png();
|
||||
std::string_view GetResource_sysid_256_png();
|
||||
std::string_view GetResource_sysid_512_png();
|
||||
} // namespace sysid
|
||||
|
||||
void Application(std::string_view saveDir) {
|
||||
// Create the wpigui (along with Dear ImGui) and Glass contexts.
|
||||
gui::CreateContext();
|
||||
glass::CreateContext();
|
||||
|
||||
// Add icons
|
||||
gui::AddIcon(sysid::GetResource_sysid_16_png());
|
||||
gui::AddIcon(sysid::GetResource_sysid_32_png());
|
||||
gui::AddIcon(sysid::GetResource_sysid_48_png());
|
||||
gui::AddIcon(sysid::GetResource_sysid_64_png());
|
||||
gui::AddIcon(sysid::GetResource_sysid_128_png());
|
||||
gui::AddIcon(sysid::GetResource_sysid_256_png());
|
||||
gui::AddIcon(sysid::GetResource_sysid_512_png());
|
||||
|
||||
glass::SetStorageName("sysid");
|
||||
glass::SetStorageDir(saveDir.empty() ? gui::GetPlatformSaveFileDir()
|
||||
: saveDir);
|
||||
|
||||
// Add messages from the global sysid logger into the Log window.
|
||||
gLogger.SetLogger([](unsigned int level, const char* file, unsigned int line,
|
||||
const char* msg) {
|
||||
const char* lvl = "";
|
||||
if (level >= wpi::WPI_LOG_CRITICAL) {
|
||||
lvl = "CRITICAL: ";
|
||||
} else if (level >= wpi::WPI_LOG_ERROR) {
|
||||
lvl = "ERROR: ";
|
||||
} else if (level >= wpi::WPI_LOG_WARNING) {
|
||||
lvl = "WARNING: ";
|
||||
} else if (level >= wpi::WPI_LOG_INFO) {
|
||||
lvl = "INFO: ";
|
||||
} else if (level >= wpi::WPI_LOG_DEBUG) {
|
||||
lvl = "DEBUG: ";
|
||||
}
|
||||
std::string filename = std::filesystem::path{file}.filename().string();
|
||||
gLog.Append(fmt::format("{}{} ({}:{})\n", lvl, msg, filename, line));
|
||||
#ifndef NDEBUG
|
||||
wpi::print(stderr, "{}{} ({}:{})\n", lvl, msg, filename, line);
|
||||
#endif
|
||||
});
|
||||
|
||||
gLogger.set_min_level(wpi::WPI_LOG_DEBUG);
|
||||
|
||||
// Initialize window manager and add views.
|
||||
auto& storage = glass::GetStorageRoot().GetChild("SysId");
|
||||
gWindowManager = std::make_unique<glass::WindowManager>(storage);
|
||||
gWindowManager->GlobalInit();
|
||||
|
||||
auto logLoader = std::make_unique<sysid::LogLoader>(storage, gLogger);
|
||||
auto dataSelector = std::make_unique<sysid::DataSelector>(storage, gLogger);
|
||||
auto analyzer = std::make_unique<sysid::Analyzer>(storage, gLogger);
|
||||
|
||||
logLoader->unload.connect([ds = dataSelector.get()] { ds->Reset(); });
|
||||
dataSelector->testdata = [_analyzer = analyzer.get(),
|
||||
ds = dataSelector.get()](auto data) {
|
||||
_analyzer->m_data = data;
|
||||
_analyzer->SetMissingTests(ds->m_missingTests);
|
||||
_analyzer->AnalyzeData();
|
||||
};
|
||||
|
||||
gLogLoaderWindow =
|
||||
gWindowManager->AddWindow("Log Loader", std::move(logLoader));
|
||||
|
||||
gDataSelectorWindow =
|
||||
gWindowManager->AddWindow("Data Selector", std::move(dataSelector));
|
||||
|
||||
gAnalyzerWindow = gWindowManager->AddWindow("Analyzer", std::move(analyzer));
|
||||
|
||||
gProgramLogWindow = gWindowManager->AddWindow(
|
||||
"Program Log", std::make_unique<glass::LogView>(&gLog));
|
||||
|
||||
// Set default positions and sizes for windows.
|
||||
|
||||
// Logger window position/size
|
||||
gLogLoaderWindow->SetDefaultPos(sysid::kLogLoaderWindowPos.x,
|
||||
sysid::kLogLoaderWindowPos.y);
|
||||
gLogLoaderWindow->SetDefaultSize(sysid::kLogLoaderWindowSize.x,
|
||||
sysid::kLogLoaderWindowSize.y);
|
||||
|
||||
// Data selector window position/size
|
||||
gDataSelectorWindow->SetDefaultPos(sysid::kDataSelectorWindowPos.x,
|
||||
sysid::kDataSelectorWindowPos.y);
|
||||
gDataSelectorWindow->SetDefaultSize(sysid::kDataSelectorWindowSize.x,
|
||||
sysid::kDataSelectorWindowSize.y);
|
||||
|
||||
// Analyzer window position/size
|
||||
gAnalyzerWindow->SetDefaultPos(sysid::kAnalyzerWindowPos.x,
|
||||
sysid::kAnalyzerWindowPos.y);
|
||||
gAnalyzerWindow->SetDefaultSize(sysid::kAnalyzerWindowSize.x,
|
||||
sysid::kAnalyzerWindowSize.y);
|
||||
|
||||
// Program log window position/size
|
||||
gProgramLogWindow->SetDefaultPos(sysid::kProgramLogWindowPos.x,
|
||||
sysid::kProgramLogWindowPos.y);
|
||||
gProgramLogWindow->SetDefaultSize(sysid::kProgramLogWindowSize.x,
|
||||
sysid::kProgramLogWindowSize.y);
|
||||
gProgramLogWindow->DisableRenamePopup();
|
||||
|
||||
// Configure save file.
|
||||
gui::ConfigurePlatformSaveFile("sysid.ini");
|
||||
|
||||
// Add menu bar.
|
||||
gui::AddLateExecute([] {
|
||||
ImGui::BeginMainMenuBar();
|
||||
gMainMenu.WorkspaceMenu();
|
||||
gui::EmitViewMenu();
|
||||
|
||||
if (ImGui::BeginMenu("Widgets")) {
|
||||
gWindowManager->DisplayMenu();
|
||||
ImGui::EndMenu();
|
||||
}
|
||||
|
||||
bool about = false;
|
||||
if (ImGui::BeginMenu("Info")) {
|
||||
if (ImGui::MenuItem("About")) {
|
||||
about = true;
|
||||
}
|
||||
ImGui::EndMenu();
|
||||
}
|
||||
|
||||
if (ImGui::BeginMenu("Docs")) {
|
||||
if (ImGui::MenuItem("Online documentation")) {
|
||||
wpi::gui::OpenURL(
|
||||
"https://docs.wpilib.org/en/stable/docs/software/pathplanning/"
|
||||
"system-identification/");
|
||||
}
|
||||
|
||||
ImGui::EndMenu();
|
||||
}
|
||||
|
||||
ImGui::EndMainMenuBar();
|
||||
|
||||
if (about) {
|
||||
ImGui::OpenPopup("About");
|
||||
about = false;
|
||||
}
|
||||
if (ImGui::BeginPopupModal("About")) {
|
||||
ImGui::Text("SysId: System Identification for Robot Mechanisms");
|
||||
ImGui::Separator();
|
||||
ImGui::Text("v%s", GetWPILibVersion());
|
||||
ImGui::Separator();
|
||||
ImGui::Text("Save location: %s", glass::GetStorageDir().c_str());
|
||||
if (ImGui::Button("Close")) {
|
||||
ImGui::CloseCurrentPopup();
|
||||
}
|
||||
ImGui::EndPopup();
|
||||
}
|
||||
});
|
||||
|
||||
gui::Initialize("System Identification", sysid::kAppWindowSize.x,
|
||||
sysid::kAppWindowSize.y);
|
||||
gui::Main();
|
||||
|
||||
glass::DestroyContext();
|
||||
gui::DestroyContext();
|
||||
}
|
||||
|
||||
#endif
|
||||
29
tools/sysid/src/main/native/cpp/Main.cpp
Normal file
29
tools/sysid/src/main/native/cpp/Main.cpp
Normal file
@@ -0,0 +1,29 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 <string_view>
|
||||
|
||||
#ifndef RUNNING_SYSID_TESTS
|
||||
|
||||
void Application(std::string_view saveDir);
|
||||
|
||||
#ifdef _WIN32
|
||||
int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine,
|
||||
int nCmdShow) {
|
||||
int argc = __argc;
|
||||
char** argv = __argv;
|
||||
#else
|
||||
int main(int argc, char** argv) {
|
||||
#endif
|
||||
std::string_view saveDir;
|
||||
if (argc == 2) {
|
||||
saveDir = argv[1];
|
||||
}
|
||||
|
||||
Application(saveDir);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
78
tools/sysid/src/main/native/cpp/Util.cpp
Normal file
78
tools/sysid/src/main/native/cpp/Util.cpp
Normal file
@@ -0,0 +1,78 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/Util.h"
|
||||
|
||||
#include <filesystem>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <IconsFontAwesome6.h>
|
||||
#include <imgui.h>
|
||||
#include <wpi/raw_ostream.h>
|
||||
|
||||
void sysid::CreateTooltip(const char* text) {
|
||||
ImGui::SameLine();
|
||||
ImGui::TextDisabled(" (?)");
|
||||
|
||||
if (ImGui::IsItemHovered()) {
|
||||
ImGui::BeginTooltip();
|
||||
ImGui::PushTextWrapPos(ImGui::GetFontSize() * 35.0f);
|
||||
ImGui::TextUnformatted(text);
|
||||
ImGui::PopTextWrapPos();
|
||||
ImGui::EndTooltip();
|
||||
}
|
||||
}
|
||||
|
||||
void sysid::CreateErrorTooltip(const char* text) {
|
||||
ImGui::SameLine();
|
||||
ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f),
|
||||
ICON_FA_TRIANGLE_EXCLAMATION);
|
||||
|
||||
if (ImGui::IsItemHovered()) {
|
||||
ImGui::BeginTooltip();
|
||||
ImGui::PushTextWrapPos(ImGui::GetFontSize() * 35.0f);
|
||||
ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s", text);
|
||||
ImGui::PopTextWrapPos();
|
||||
ImGui::EndTooltip();
|
||||
}
|
||||
}
|
||||
|
||||
void sysid::CreateErrorPopup(bool& isError, std::string_view errorMessage) {
|
||||
if (isError) {
|
||||
ImGui::OpenPopup("Exception Caught!");
|
||||
}
|
||||
|
||||
// Handle exceptions.
|
||||
ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
|
||||
if (ImGui::BeginPopupModal("Exception Caught!")) {
|
||||
ImGui::PushTextWrapPos(0.0f);
|
||||
ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s",
|
||||
errorMessage.data());
|
||||
ImGui::PopTextWrapPos();
|
||||
if (ImGui::Button("Close")) {
|
||||
ImGui::CloseCurrentPopup();
|
||||
isError = false;
|
||||
}
|
||||
ImGui::EndPopup();
|
||||
}
|
||||
}
|
||||
|
||||
void sysid::SaveFile(std::string_view contents,
|
||||
const std::filesystem::path& path) {
|
||||
// Create the path if it doesn't already exist.
|
||||
std::filesystem::create_directories(path.root_directory());
|
||||
|
||||
// Open a fd_ostream to write to file.
|
||||
std::error_code ec;
|
||||
// NOLINTNEXTLINE(build/include_what_you_use)
|
||||
wpi::raw_fd_ostream ostream{path.string(), ec};
|
||||
|
||||
// Check error code.
|
||||
if (ec) {
|
||||
throw std::runtime_error("Cannot write to file: " + ec.message());
|
||||
}
|
||||
|
||||
// Write contents.
|
||||
ostream << contents;
|
||||
}
|
||||
290
tools/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
Normal file
290
tools/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
Normal file
@@ -0,0 +1,290 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/analysis/AnalysisManager.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <units/angle.h>
|
||||
#include <wpi/MathExtras.h>
|
||||
#include <wpi/StringExtras.h>
|
||||
#include <wpi/StringMap.h>
|
||||
|
||||
#include "sysid/analysis/FeedforwardAnalysis.h"
|
||||
#include "sysid/analysis/FilteringUtils.h"
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
static double Lerp(units::second_t time,
|
||||
std::vector<MotorData::Run::Sample<double>>& data) {
|
||||
auto next = std::find_if(data.begin(), data.end(), [&](const auto& entry) {
|
||||
return entry.time > time;
|
||||
});
|
||||
|
||||
if (next == data.begin()) {
|
||||
next++;
|
||||
}
|
||||
|
||||
if (next == data.end()) {
|
||||
next--;
|
||||
}
|
||||
|
||||
const auto prev = next - 1;
|
||||
|
||||
return wpi::Lerp(prev->measurement, next->measurement,
|
||||
(time - prev->time) / (next->time - prev->time));
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a raw data vector into a PreparedData vector with only the
|
||||
* timestamp, voltage, position, and velocity fields filled out.
|
||||
*
|
||||
* @tparam S The size of the arrays in the raw data vector
|
||||
* @tparam Timestamp The index of the Timestamp data in the raw data vector
|
||||
* arrays
|
||||
* @tparam Voltage The index of the Voltage data in the raw data vector arrays
|
||||
* @tparam Position The index of the Position data in the raw data vector arrays
|
||||
* @tparam Velocity The index of the Velocity data in the raw data vector arrays
|
||||
*
|
||||
* @param data A raw data vector
|
||||
*
|
||||
* @return A PreparedData vector
|
||||
*/
|
||||
static std::vector<PreparedData> ConvertToPrepared(const MotorData& data) {
|
||||
std::vector<PreparedData> prepared;
|
||||
// assume we've selected down to a single contiguous run by this point
|
||||
auto run = data.runs[0];
|
||||
|
||||
for (int i = 0; i < static_cast<int>(run.voltage.size()) - 1; ++i) {
|
||||
const auto& currentVoltage = run.voltage[i];
|
||||
const auto& nextVoltage = run.voltage[i + 1];
|
||||
|
||||
auto currentPosition = Lerp(currentVoltage.time, run.position);
|
||||
|
||||
auto currentVelocity = Lerp(currentVoltage.time, run.velocity);
|
||||
|
||||
prepared.emplace_back(PreparedData{currentVoltage.time,
|
||||
currentVoltage.measurement.value(),
|
||||
currentPosition, currentVelocity,
|
||||
nextVoltage.time - currentVoltage.time});
|
||||
}
|
||||
|
||||
return prepared;
|
||||
}
|
||||
|
||||
/**
|
||||
* To preserve a raw copy of the data, this method saves a raw version
|
||||
* in the dataset StringMap where the key of the raw data starts with "raw-"
|
||||
* before the name of the original data.
|
||||
*
|
||||
* @tparam S The size of the array data that's being used
|
||||
*
|
||||
* @param dataset A reference to the dataset being used
|
||||
*/
|
||||
static void CopyRawData(wpi::StringMap<MotorData>* dataset) {
|
||||
auto& data = *dataset;
|
||||
// Loads the Raw Data
|
||||
for (auto& it : data) {
|
||||
auto& key = it.first;
|
||||
auto& motorData = it.second;
|
||||
|
||||
if (!wpi::contains(key, "raw")) {
|
||||
data[fmt::format("raw-{}", key)] = motorData;
|
||||
data[fmt::format("original-raw-{}", key)] = motorData;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Assigns the combines the various datasets into a single one for analysis.
|
||||
*
|
||||
* @param slowForward The slow forward dataset
|
||||
* @param slowBackward The slow backward dataset
|
||||
* @param fastForward The fast forward dataset
|
||||
* @param fastBackward The fast backward dataset
|
||||
*/
|
||||
static Storage CombineDatasets(const std::vector<PreparedData>& slowForward,
|
||||
const std::vector<PreparedData>& slowBackward,
|
||||
const std::vector<PreparedData>& fastForward,
|
||||
const std::vector<PreparedData>& fastBackward) {
|
||||
return Storage{slowForward, slowBackward, fastForward, fastBackward};
|
||||
}
|
||||
|
||||
void AnalysisManager::PrepareGeneralData() {
|
||||
wpi::StringMap<std::vector<PreparedData>> preparedData;
|
||||
|
||||
WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
|
||||
|
||||
WPI_INFO(m_logger, "{}", "Copying raw data.");
|
||||
CopyRawData(&m_data.motorData);
|
||||
|
||||
WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct.");
|
||||
// Convert data to PreparedData structs
|
||||
for (auto& it : m_data.motorData) {
|
||||
auto key = it.first;
|
||||
preparedData[key] = ConvertToPrepared(m_data.motorData[key]);
|
||||
WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size());
|
||||
}
|
||||
|
||||
// Store the original datasets
|
||||
m_originalDataset =
|
||||
CombineDatasets(preparedData["original-raw-quasistatic-forward"],
|
||||
preparedData["original-raw-quasistatic-reverse"],
|
||||
preparedData["original-raw-dynamic-forward"],
|
||||
preparedData["original-raw-dynamic-reverse"]);
|
||||
|
||||
WPI_INFO(m_logger, "{}", "Initial trimming and filtering.");
|
||||
sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
|
||||
m_velocityDelays, m_minStepTime, m_maxStepTime,
|
||||
m_data.distanceUnit);
|
||||
|
||||
WPI_INFO(m_logger, "{}", m_minStepTime);
|
||||
WPI_INFO(m_logger, "{}", m_maxStepTime);
|
||||
|
||||
WPI_INFO(m_logger, "{}", "Acceleration filtering.");
|
||||
sysid::AccelFilter(&preparedData);
|
||||
|
||||
WPI_INFO(m_logger, "{}", "Storing datasets.");
|
||||
// Store the raw datasets
|
||||
m_rawDataset = CombineDatasets(preparedData["raw-quasistatic-forward"],
|
||||
preparedData["raw-quasistatic-reverse"],
|
||||
preparedData["raw-dynamic-forward"],
|
||||
preparedData["raw-dynamic-reverse"]);
|
||||
|
||||
// Store the filtered datasets
|
||||
m_filteredDataset = CombineDatasets(
|
||||
preparedData["quasistatic-forward"], preparedData["quasistatic-reverse"],
|
||||
preparedData["dynamic-forward"], preparedData["dynamic-reverse"]);
|
||||
|
||||
m_startTimes = {preparedData["raw-quasistatic-forward"][0].timestamp,
|
||||
preparedData["raw-quasistatic-reverse"][0].timestamp,
|
||||
preparedData["raw-dynamic-forward"][0].timestamp,
|
||||
preparedData["raw-dynamic-reverse"][0].timestamp};
|
||||
}
|
||||
|
||||
AnalysisManager::AnalysisManager(Settings& settings, wpi::Logger& logger)
|
||||
: m_logger{logger}, m_settings{settings} {}
|
||||
|
||||
AnalysisManager::AnalysisManager(TestData data, Settings& settings,
|
||||
wpi::Logger& logger)
|
||||
: m_data{std::move(data)}, m_logger{logger}, m_settings{settings} {
|
||||
// Reset settings for Dynamic Test Limits
|
||||
m_settings.stepTestDuration = 0_s;
|
||||
m_settings.velocityThreshold = std::numeric_limits<double>::infinity();
|
||||
}
|
||||
|
||||
void AnalysisManager::PrepareData() {
|
||||
// WPI_INFO(m_logger, "Preparing {} data", m_data.mechanismType.name);
|
||||
|
||||
PrepareGeneralData();
|
||||
|
||||
WPI_INFO(m_logger, "{}", "Finished Preparing Data");
|
||||
}
|
||||
|
||||
AnalysisManager::FeedforwardGains AnalysisManager::CalculateFeedforward() {
|
||||
if (m_filteredDataset.empty()) {
|
||||
throw sysid::InvalidDataError(
|
||||
"There is no data to perform gain calculation on.");
|
||||
}
|
||||
|
||||
WPI_INFO(m_logger, "{}", "Calculating Gains");
|
||||
// Calculate feedforward gains from the data.
|
||||
const auto& analysisType = m_data.mechanismType;
|
||||
const auto& ff =
|
||||
sysid::CalculateFeedforwardGains(GetFilteredData(), analysisType, false);
|
||||
|
||||
const auto& Ks = ff.coeffs[0];
|
||||
FeedforwardGain KsGain = {
|
||||
.gain = Ks, .descriptor = "Voltage needed to overcome static friction."};
|
||||
if (Ks < 0) {
|
||||
KsGain.isValidGain = false;
|
||||
KsGain.errorMessage = fmt::format(
|
||||
"Calculated Ks gain of: {0:.3f} is erroneous! Ks should be >= 0.", Ks);
|
||||
}
|
||||
|
||||
const auto& Kv = ff.coeffs[1];
|
||||
FeedforwardGain KvGain = {
|
||||
.gain = Kv,
|
||||
.descriptor =
|
||||
"Voltage needed to hold/cruise at a constant velocity while "
|
||||
"overcoming the counter-electromotive force and any additional "
|
||||
"friction."};
|
||||
if (Kv < 0) {
|
||||
KvGain.isValidGain = false;
|
||||
KvGain.errorMessage = fmt::format(
|
||||
"Calculated Kv gain of: {0:.3f} is erroneous! Kv should be >= 0.", Kv);
|
||||
}
|
||||
|
||||
const auto& Ka = ff.coeffs[2];
|
||||
FeedforwardGain KaGain = {
|
||||
.gain = Ka,
|
||||
.descriptor =
|
||||
"Voltage needed to induce a given acceleration in the motor shaft."};
|
||||
if (Ka <= 0) {
|
||||
KaGain.isValidGain = false;
|
||||
KaGain.errorMessage = fmt::format(
|
||||
"Calculated Ka gain of: {0:.3f} is erroneous! Ka should be > 0.", Ka);
|
||||
}
|
||||
|
||||
if (analysisType == analysis::kSimple) {
|
||||
return FeedforwardGains{
|
||||
.olsResult = ff, .Ks = KsGain, .Kv = KvGain, .Ka = KaGain};
|
||||
}
|
||||
|
||||
if (analysisType == analysis::kElevator || analysisType == analysis::kArm) {
|
||||
const auto& Kg = ff.coeffs[3];
|
||||
FeedforwardGain KgGain = {
|
||||
Kg, "Voltage needed to counteract the force of gravity."};
|
||||
if (Kg < 0) {
|
||||
KgGain.isValidGain = false;
|
||||
KgGain.errorMessage = fmt::format(
|
||||
"Calculated Kg gain of: {0:.3f} is erroneous! Kg should be >= 0.",
|
||||
Ka);
|
||||
}
|
||||
|
||||
// Elevator analysis only requires Kg
|
||||
if (analysisType == analysis::kElevator) {
|
||||
return FeedforwardGains{.olsResult = ff,
|
||||
.Ks = KsGain,
|
||||
.Kv = KvGain,
|
||||
.Ka = KaGain,
|
||||
.Kg = KgGain};
|
||||
} else {
|
||||
// Arm analysis requires Kg and an angle offset
|
||||
FeedforwardGain offset = {
|
||||
.gain = ff.coeffs[4],
|
||||
.descriptor =
|
||||
"This is the angle offset which, when added to the angle "
|
||||
"measurement, zeroes it out when the arm is horizontal. This is "
|
||||
"needed for the arm feedforward to work."};
|
||||
return FeedforwardGains{ff, KsGain, KvGain, KaGain, KgGain, offset};
|
||||
}
|
||||
}
|
||||
|
||||
return FeedforwardGains{.olsResult = ff};
|
||||
}
|
||||
|
||||
sysid::FeedbackGains AnalysisManager::CalculateFeedback(
|
||||
const FeedforwardGain& Kv, const FeedforwardGain& Ka) {
|
||||
FeedbackGains fb;
|
||||
if (m_settings.type == FeedbackControllerLoopType::kPosition) {
|
||||
fb = sysid::CalculatePositionFeedbackGains(
|
||||
m_settings.preset, m_settings.lqr, Kv.gain, Ka.gain);
|
||||
} else {
|
||||
fb = sysid::CalculateVelocityFeedbackGains(
|
||||
m_settings.preset, m_settings.lqr, Kv.gain, Ka.gain);
|
||||
}
|
||||
|
||||
return fb;
|
||||
}
|
||||
|
||||
void AnalysisManager::OverrideUnits(std::string_view unit) {
|
||||
m_data.distanceUnit = unit;
|
||||
}
|
||||
|
||||
void AnalysisManager::ResetUnitsFromJSON() {}
|
||||
64
tools/sysid/src/main/native/cpp/analysis/ArmSim.cpp
Normal file
64
tools/sysid/src/main/native/cpp/analysis/ArmSim.cpp
Normal file
@@ -0,0 +1,64 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/analysis/ArmSim.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
#include <frc/system/NumericalIntegration.h>
|
||||
#include <wpi/MathExtras.h>
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
ArmSim::ArmSim(double Ks, double Kv, double Ka, double Kg, double offset,
|
||||
double initialPosition, double initialVelocity)
|
||||
// u = Ks sgn(x) + Kv x + Ka a + Kg cos(theta)
|
||||
// Ka a = u - Ks sgn(x) - Kv x - Kg cos(theta)
|
||||
// a = 1/Ka u - Ks/Ka sgn(x) - Kv/Ka x - Kg/Ka cos(theta)
|
||||
// a = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(theta)
|
||||
// a = Ax + Bu + c sgn(x) + d cos(theta)
|
||||
: m_A{-Kv / Ka},
|
||||
m_B{1.0 / Ka},
|
||||
m_c{-Ks / Ka},
|
||||
m_d{-Kg / Ka},
|
||||
m_offset{offset} {
|
||||
Reset(initialPosition, initialVelocity);
|
||||
}
|
||||
|
||||
void ArmSim::Update(units::volt_t voltage, units::second_t dt) {
|
||||
// Returns arm acceleration under gravity
|
||||
auto f = [=, this](
|
||||
const Eigen::Vector<double, 2>& x,
|
||||
const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 2> {
|
||||
return Eigen::Vector<double, 2>{
|
||||
x(1), (m_A * x.block<1, 1>(1, 0) + m_B * u + m_c * wpi::sgn(x(1)) +
|
||||
m_d * std::cos(x(0) + m_offset))(0)};
|
||||
};
|
||||
|
||||
// Max error is large because an accurate sim isn't as important as the sim
|
||||
// finishing in a timely manner. Otherwise, the timestep can become absurdly
|
||||
// small for ill-conditioned data (e.g., high velocities with sharp spikes in
|
||||
// acceleration).
|
||||
Eigen::Vector<double, 1> u{voltage.value()};
|
||||
m_x = frc::RKDP(f, m_x, u, dt, 0.25);
|
||||
}
|
||||
|
||||
double ArmSim::GetPosition() const {
|
||||
return m_x(0);
|
||||
}
|
||||
|
||||
double ArmSim::GetVelocity() const {
|
||||
return m_x(1);
|
||||
}
|
||||
|
||||
double ArmSim::GetAcceleration(units::volt_t voltage) const {
|
||||
Eigen::Vector<double, 1> u{voltage.value()};
|
||||
return (m_A * m_x.block<1, 1>(1, 0) + m_B * u +
|
||||
m_c * wpi::sgn(GetVelocity()) + m_d * std::cos(m_x(0) + m_offset))(0);
|
||||
}
|
||||
|
||||
void ArmSim::Reset(double position, double velocity) {
|
||||
m_x = Eigen::Vector<double, 2>{position, velocity};
|
||||
}
|
||||
50
tools/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp
Normal file
50
tools/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp
Normal file
@@ -0,0 +1,50 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/analysis/ElevatorSim.h"
|
||||
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
#include <frc/system/Discretization.h>
|
||||
#include <wpi/MathExtras.h>
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
ElevatorSim::ElevatorSim(double Ks, double Kv, double Ka, double Kg,
|
||||
double initialPosition, double initialVelocity)
|
||||
// dx/dt = Ax + Bu + c sgn(x) + d
|
||||
: m_A{{0.0, 1.0}, {0.0, -Kv / Ka}},
|
||||
m_B{0.0, 1.0 / Ka},
|
||||
m_c{0.0, -Ks / Ka},
|
||||
m_d{0.0, -Kg / Ka} {
|
||||
Reset(initialPosition, initialVelocity);
|
||||
}
|
||||
|
||||
void ElevatorSim::Update(units::volt_t voltage, units::second_t dt) {
|
||||
Eigen::Vector<double, 1> u{voltage.value()};
|
||||
|
||||
// Given dx/dt = Ax + Bu + c sgn(x) + d,
|
||||
// x_k+1 = e^(AT) x_k + A^-1 (e^(AT) - 1) (Bu + c sgn(x) + d)
|
||||
Eigen::Matrix<double, 2, 2> Ad;
|
||||
Eigen::Matrix<double, 2, 1> Bd;
|
||||
frc::DiscretizeAB<2, 1>(m_A, m_B, dt, &Ad, &Bd);
|
||||
m_x = Ad * m_x + Bd * u +
|
||||
Bd * m_B.householderQr().solve(m_c * wpi::sgn(GetVelocity()) + m_d);
|
||||
}
|
||||
|
||||
double ElevatorSim::GetPosition() const {
|
||||
return m_x(0);
|
||||
}
|
||||
|
||||
double ElevatorSim::GetVelocity() const {
|
||||
return m_x(1);
|
||||
}
|
||||
|
||||
double ElevatorSim::GetAcceleration(units::volt_t voltage) const {
|
||||
Eigen::Vector<double, 1> u{voltage.value()};
|
||||
return (m_A * m_x + m_B * u + m_c * wpi::sgn(GetVelocity()) + m_d)(1);
|
||||
}
|
||||
|
||||
void ElevatorSim::Reset(double position, double velocity) {
|
||||
m_x = Eigen::Vector<double, 2>{position, velocity};
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/analysis/FeedbackAnalysis.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <frc/controller/LinearQuadraticRegulator.h>
|
||||
#include <frc/system/LinearSystem.h>
|
||||
#include <frc/system/plant/LinearSystemId.h>
|
||||
#include <units/acceleration.h>
|
||||
#include <units/velocity.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
#include "sysid/analysis/FeedbackControllerPreset.h"
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
using Kv_t = decltype(1_V / 1_mps);
|
||||
using Ka_t = decltype(1_V / 1_mps_sq);
|
||||
using Matrix1d = Eigen::Matrix<double, 1, 1>;
|
||||
|
||||
FeedbackGains sysid::CalculatePositionFeedbackGains(
|
||||
const FeedbackControllerPreset& preset, const LQRParameters& params,
|
||||
double Kv, double Ka) {
|
||||
if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
|
||||
return {0.0, 0.0};
|
||||
}
|
||||
|
||||
// If acceleration for position control requires no effort, velocity becomes
|
||||
// an input. We choose an appropriate model in this case to avoid numerical
|
||||
// instabilities in the LQR.
|
||||
if (std::abs(Ka) < 1e-7) {
|
||||
// System has position state and velocity input
|
||||
frc::LinearSystem<1, 1, 1> system{Matrix1d{0.0}, Matrix1d{1.0},
|
||||
Matrix1d{1.0}, Matrix1d{0.0}};
|
||||
|
||||
frc::LinearQuadraticRegulator<1, 1> controller{
|
||||
system, {params.qp}, {params.r}, preset.period};
|
||||
controller.LatencyCompensate(system, preset.period,
|
||||
preset.measurementDelay);
|
||||
|
||||
return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
|
||||
}
|
||||
|
||||
auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meters>(
|
||||
Kv_t{Kv}, Ka_t{Ka});
|
||||
|
||||
frc::LinearQuadraticRegulator<2, 1> controller{
|
||||
system, {params.qp, params.qv}, {params.r}, preset.period};
|
||||
controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
|
||||
|
||||
return {controller.K(0, 0) * preset.outputConversionFactor,
|
||||
controller.K(0, 1) * preset.outputConversionFactor /
|
||||
(preset.normalized ? 1 : units::second_t{preset.period}.value())};
|
||||
}
|
||||
|
||||
FeedbackGains sysid::CalculateVelocityFeedbackGains(
|
||||
const FeedbackControllerPreset& preset, const LQRParameters& params,
|
||||
double Kv, double Ka, double encFactor) {
|
||||
if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
|
||||
return {0.0, 0.0};
|
||||
}
|
||||
|
||||
// If acceleration for velocity control requires no effort, the feedback
|
||||
// control gains approach zero. We special-case it here to avoid numerical
|
||||
// instabilities in LQR.
|
||||
if (std::abs(Ka) < 1E-7) {
|
||||
return {0.0, 0.0};
|
||||
}
|
||||
|
||||
auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
|
||||
Kv_t{Kv}, Ka_t{Ka});
|
||||
frc::LinearQuadraticRegulator<1, 1> controller{
|
||||
system, {params.qv}, {params.r}, preset.period};
|
||||
controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
|
||||
|
||||
return {controller.K(0, 0) * preset.outputConversionFactor /
|
||||
(preset.outputVelocityTimeFactor * encFactor),
|
||||
0.0};
|
||||
}
|
||||
274
tools/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
Normal file
274
tools/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp
Normal file
@@ -0,0 +1,274 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/analysis/FeedforwardAnalysis.h"
|
||||
|
||||
#include <array>
|
||||
#include <bitset>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <fmt/format.h>
|
||||
#include <fmt/ranges.h>
|
||||
#include <units/math.h>
|
||||
#include <units/time.h>
|
||||
|
||||
#include "sysid/analysis/OLS.h"
|
||||
|
||||
namespace sysid {
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* @param X Vector representation of X in y = Xβ.
|
||||
* @param y Vector representation of y in y = Xβ.
|
||||
*/
|
||||
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];
|
||||
|
||||
// Set the velocity term (for α)
|
||||
X(sample, 0) = pt.velocity;
|
||||
|
||||
// Set the voltage term (for β)
|
||||
X(sample, 1) = pt.voltage;
|
||||
|
||||
// Set the intercept term (for γ)
|
||||
X(sample, 2) = std::copysign(1, pt.velocity);
|
||||
|
||||
// Set test-specific variables
|
||||
if (type == analysis::kElevator) {
|
||||
// Set the gravity term (for δ)
|
||||
X(sample, 3) = 1.0;
|
||||
} else if (type == analysis::kArm) {
|
||||
// Set the cosine and sine terms (for δ and ε)
|
||||
X(sample, 3) = pt.cos;
|
||||
X(sample, 4) = pt.sin;
|
||||
}
|
||||
|
||||
// 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 nonzero eigenvalues for good fit
|
||||
for (int row = 0; row < eigvals.rows(); ++row) {
|
||||
// Find row of eigenvector with largest magnitude. This determines the
|
||||
// primary regression variable that corresponds to the eigenvalue.
|
||||
int maxIndex;
|
||||
double maxCoeff = eigvecs.col(row).cwiseAbs().maxCoeff(&maxIndex);
|
||||
|
||||
// Check whether the eigenvector component along the regression variable's
|
||||
// direction is below the threshold. If it is, the regression variable's fit
|
||||
// is bad.
|
||||
if (std::abs(eigvals(row) * maxCoeff) <= threshold) {
|
||||
// Fit for α is bad
|
||||
if (maxIndex == 0) {
|
||||
// Affects Kv
|
||||
badGains.set(1);
|
||||
}
|
||||
|
||||
// Fit for β is bad
|
||||
if (maxIndex == 1) {
|
||||
// Affects all gains
|
||||
badGains.set();
|
||||
break;
|
||||
}
|
||||
|
||||
// Fit for γ is bad
|
||||
if (maxIndex == 2) {
|
||||
// Affects Ks
|
||||
badGains.set(0);
|
||||
}
|
||||
|
||||
// Fit for δ is bad
|
||||
if (maxIndex == 3) {
|
||||
if (type == analysis::kElevator) {
|
||||
// Affects Kg
|
||||
badGains.set(3);
|
||||
} else if (type == analysis::kArm) {
|
||||
// Affects Kg and offset
|
||||
badGains.set(3);
|
||||
badGains.set(4);
|
||||
}
|
||||
}
|
||||
|
||||
// Fit for ε is bad
|
||||
if (maxIndex == 4) {
|
||||
// Affects Kg and offset
|
||||
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,
|
||||
bool throwOnBadData) {
|
||||
// Iterate through the data and add it to our raw vector.
|
||||
const auto& [slowForward, slowBackward, fastForward, fastBackward] = data;
|
||||
|
||||
const auto size = slowForward.size() + slowBackward.size() +
|
||||
fastForward.size() + fastBackward.size();
|
||||
|
||||
// Create a raw vector of doubles with our data in it.
|
||||
Eigen::MatrixXd X{size, type.independentVariables};
|
||||
Eigen::VectorXd y{size};
|
||||
|
||||
int rowOffset = 0;
|
||||
PopulateOLSData(slowForward, type,
|
||||
X.block(rowOffset, 0, slowForward.size(), X.cols()),
|
||||
y.segment(rowOffset, slowForward.size()));
|
||||
|
||||
rowOffset += slowForward.size();
|
||||
PopulateOLSData(slowBackward, type,
|
||||
X.block(rowOffset, 0, slowBackward.size(), X.cols()),
|
||||
y.segment(rowOffset, slowBackward.size()));
|
||||
|
||||
rowOffset += slowBackward.size();
|
||||
PopulateOLSData(fastForward, type,
|
||||
X.block(rowOffset, 0, fastForward.size(), X.cols()),
|
||||
y.segment(rowOffset, fastForward.size()));
|
||||
|
||||
rowOffset += fastForward.size();
|
||||
PopulateOLSData(fastBackward, type,
|
||||
X.block(rowOffset, 0, fastBackward.size(), X.cols()),
|
||||
y.segment(rowOffset, fastBackward.size()));
|
||||
|
||||
// Check quality of collected data
|
||||
if (throwOnBadData) {
|
||||
CheckOLSDataQuality(X, type);
|
||||
}
|
||||
|
||||
std::vector<double> gains;
|
||||
gains.reserve(X.rows());
|
||||
|
||||
auto ols = OLS(X, y);
|
||||
|
||||
// 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)
|
||||
// + Kg/Ka sin(offset) sin(angle)
|
||||
// dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)
|
||||
|
||||
// δ = -Kg/Ka cos(offset)
|
||||
// ε = Kg/Ka sin(offset)
|
||||
double δ = ols.coeffs[3];
|
||||
double ε = ols.coeffs[4];
|
||||
|
||||
// Kg = hypot(δ, ε)/β
|
||||
// offset = atan2(ε, -δ)
|
||||
gains.emplace_back(std::hypot(δ, ε) / β);
|
||||
gains.emplace_back(std::atan2(ε, -δ));
|
||||
}
|
||||
}
|
||||
|
||||
// Gains are Ks, Kv, Ka, Kg (elevator/arm only), offset (arm only)
|
||||
return OLSResult{gains, ols.rSquared, ols.rmse};
|
||||
}
|
||||
|
||||
} // namespace sysid
|
||||
443
tools/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
Normal file
443
tools/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
Normal file
@@ -0,0 +1,443 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/analysis/FilteringUtils.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
#include <numbers>
|
||||
#include <numeric>
|
||||
#include <string>
|
||||
#include <tuple>
|
||||
#include <vector>
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <frc/filter/LinearFilter.h>
|
||||
#include <frc/filter/MedianFilter.h>
|
||||
#include <units/math.h>
|
||||
#include <wpi/MathExtras.h>
|
||||
#include <wpi/StringExtras.h>
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
/**
|
||||
* Helper function that throws if it detects that the data vector is too small
|
||||
* for an operation of a certain window size.
|
||||
*
|
||||
* @param data The data that is being used.
|
||||
* @param window The window size for the operation.
|
||||
* @param operation The operation we're checking the size for (for error
|
||||
* throwing purposes).
|
||||
*/
|
||||
static void CheckSize(const std::vector<PreparedData>& data, size_t window,
|
||||
std::string_view operation) {
|
||||
if (data.size() < window) {
|
||||
throw sysid::InvalidDataError(
|
||||
fmt::format("Not enough data to run {} which has a window size of {}.",
|
||||
operation, window));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Helper function that determines if a certain key is storing raw data.
|
||||
*
|
||||
* @param key The key of the dataset
|
||||
*
|
||||
* @return True, if the key corresponds to a raw dataset.
|
||||
*/
|
||||
static bool IsRaw(std::string_view key) {
|
||||
return wpi::contains(key, "raw") && !wpi::contains(key, "original");
|
||||
}
|
||||
|
||||
/**
|
||||
* Helper function that determines if a certain key is storing filtered data.
|
||||
*
|
||||
* @param key The key of the dataset
|
||||
*
|
||||
* @return True, if the key corresponds to a filtered dataset.
|
||||
*/
|
||||
static bool IsFiltered(std::string_view key) {
|
||||
return !wpi::contains(key, "raw") && !wpi::contains(key, "original");
|
||||
}
|
||||
|
||||
/**
|
||||
* Fills in the rest of the PreparedData Structs for a PreparedData Vector.
|
||||
*
|
||||
* @param data A reference to a vector of the raw data.
|
||||
* @param unit The units that the data is in (rotations, radians, or degrees)
|
||||
* for arm mechanisms.
|
||||
*/
|
||||
static void PrepareMechData(std::vector<PreparedData>* data,
|
||||
std::string_view unit = "") {
|
||||
constexpr size_t kWindow = 3;
|
||||
|
||||
CheckSize(*data, kWindow, "Acceleration Calculation");
|
||||
|
||||
// Calculates the cosine of the position data for single jointed arm analysis
|
||||
for (size_t i = 0; i < data->size(); ++i) {
|
||||
auto& pt = data->at(i);
|
||||
|
||||
double cos = 0.0;
|
||||
double sin = 0.0;
|
||||
if (unit == "Radians") {
|
||||
cos = std::cos(pt.position);
|
||||
sin = std::sin(pt.position);
|
||||
} else if (unit == "Degrees") {
|
||||
cos = std::cos(pt.position * std::numbers::pi / 180.0);
|
||||
sin = std::sin(pt.position * std::numbers::pi / 180.0);
|
||||
} else if (unit == "Rotations") {
|
||||
cos = std::cos(pt.position * 2 * std::numbers::pi);
|
||||
sin = std::sin(pt.position * 2 * std::numbers::pi);
|
||||
}
|
||||
pt.cos = cos;
|
||||
pt.sin = sin;
|
||||
}
|
||||
|
||||
auto derivative =
|
||||
CentralFiniteDifference<1, kWindow>(GetMeanTimeDelta(*data));
|
||||
|
||||
// Load the derivative filter with the first value for accurate initial
|
||||
// behavior
|
||||
for (size_t i = 0; i < kWindow; ++i) {
|
||||
derivative.Calculate(data->at(0).velocity);
|
||||
}
|
||||
|
||||
for (size_t i = (kWindow - 1) / 2; i < data->size(); ++i) {
|
||||
data->at(i - (kWindow - 1) / 2).acceleration =
|
||||
derivative.Calculate(data->at(i).velocity);
|
||||
}
|
||||
|
||||
// Fill in accelerations past end of derivative filter
|
||||
for (size_t i = data->size() - (kWindow - 1) / 2; i < data->size(); ++i) {
|
||||
data->at(i).acceleration = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
std::tuple<units::second_t, units::second_t, units::second_t>
|
||||
sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
|
||||
AnalysisManager::Settings* settings,
|
||||
units::second_t minStepTime,
|
||||
units::second_t maxStepTime) {
|
||||
auto voltageBegins =
|
||||
std::find_if(data->begin(), data->end(),
|
||||
[](auto& datum) { return std::abs(datum.voltage) > 0; });
|
||||
|
||||
units::second_t firstTimestamp = voltageBegins->timestamp;
|
||||
double firstPosition = voltageBegins->position;
|
||||
|
||||
auto motionBegins = std::find_if(
|
||||
data->begin(), data->end(), [settings, firstPosition](auto& datum) {
|
||||
return std::abs(datum.position - firstPosition) >
|
||||
(settings->velocityThreshold * datum.dt.value());
|
||||
});
|
||||
|
||||
units::second_t positionDelay;
|
||||
if (motionBegins != data->end()) {
|
||||
positionDelay = motionBegins->timestamp - firstTimestamp;
|
||||
} else {
|
||||
positionDelay = 0_s;
|
||||
}
|
||||
|
||||
auto maxAccel = std::max_element(
|
||||
data->begin(), data->end(), [](const auto& a, const auto& b) {
|
||||
// Since we don't know if its a forward or backwards test here, we use
|
||||
// the sign of each point's velocity to determine how to compare their
|
||||
// accelerations.
|
||||
return wpi::sgn(a.velocity) * a.acceleration <
|
||||
wpi::sgn(b.velocity) * b.acceleration;
|
||||
});
|
||||
|
||||
// Current limiting can delay onset of the peak acceleration, so we need to
|
||||
// find the first acceleration *near* the max. Magic number tolerance here
|
||||
// because this whole file is tech debt already
|
||||
auto accelBegins = std::find_if(
|
||||
data->begin(), data->end(), [&maxAccel](const auto& measurement) {
|
||||
return wpi::sgn(measurement.velocity) * measurement.acceleration >
|
||||
0.8 * wpi::sgn(maxAccel->velocity) * maxAccel->acceleration;
|
||||
});
|
||||
|
||||
units::second_t velocityDelay;
|
||||
if (accelBegins != data->end()) {
|
||||
velocityDelay = accelBegins->timestamp - firstTimestamp;
|
||||
|
||||
// Trim data before max acceleration
|
||||
data->erase(data->begin(), maxAccel);
|
||||
} else {
|
||||
velocityDelay = 0_s;
|
||||
}
|
||||
|
||||
minStepTime = std::min(data->at(0).timestamp - firstTimestamp, minStepTime);
|
||||
|
||||
// If step test duration not yet specified, calculate default
|
||||
if (settings->stepTestDuration == 0_s) {
|
||||
// Find maximum speed reached
|
||||
const auto maxSpeed =
|
||||
GetMaxSpeed(*data, [](auto&& pt) { return pt.velocity; });
|
||||
// Find place where 90% of maximum speed exceeded
|
||||
auto endIt = std::find_if(
|
||||
data->begin(), data->end(), [&](const PreparedData& entry) {
|
||||
return std::abs(entry.velocity) > maxSpeed * 0.9;
|
||||
});
|
||||
|
||||
if (endIt != data->end()) {
|
||||
settings->stepTestDuration =
|
||||
std::min(endIt->timestamp - data->front().timestamp + minStepTime,
|
||||
maxStepTime);
|
||||
}
|
||||
}
|
||||
|
||||
// Find first entry greater than the step test duration
|
||||
auto maxIt =
|
||||
std::find_if(data->begin(), data->end(), [&](PreparedData entry) {
|
||||
return entry.timestamp - data->front().timestamp >
|
||||
settings->stepTestDuration;
|
||||
});
|
||||
|
||||
// Trim data beyond desired step test duration
|
||||
if (maxIt != data->end()) {
|
||||
data->erase(maxIt, data->end());
|
||||
}
|
||||
|
||||
return std::make_tuple(minStepTime, positionDelay, velocityDelay);
|
||||
}
|
||||
|
||||
double sysid::GetNoiseFloor(
|
||||
const std::vector<PreparedData>& data, int window,
|
||||
std::function<double(const PreparedData&)> accessorFunction) {
|
||||
double sum = 0.0;
|
||||
size_t step = window / 2;
|
||||
auto averageFilter = frc::LinearFilter<double>::MovingAverage(window);
|
||||
for (size_t i = 0; i < data.size(); i++) {
|
||||
double mean = averageFilter.Calculate(accessorFunction(data[i]));
|
||||
if (i >= step) {
|
||||
sum += std::pow(accessorFunction(data[i - step]) - mean, 2);
|
||||
}
|
||||
}
|
||||
return std::sqrt(sum / (data.size() - step));
|
||||
}
|
||||
|
||||
double sysid::GetMaxSpeed(
|
||||
const std::vector<PreparedData>& data,
|
||||
std::function<double(const PreparedData&)> accessorFunction) {
|
||||
double max = 0.0;
|
||||
for (size_t i = 0; i < data.size(); i++) {
|
||||
max = std::max(max, std::abs(accessorFunction(data[i])));
|
||||
}
|
||||
return max;
|
||||
}
|
||||
|
||||
units::second_t sysid::GetMeanTimeDelta(const std::vector<PreparedData>& data) {
|
||||
std::vector<units::second_t> dts;
|
||||
|
||||
for (const auto& pt : data) {
|
||||
if (pt.dt > 0_s && pt.dt < 500_ms) {
|
||||
dts.emplace_back(pt.dt);
|
||||
}
|
||||
}
|
||||
|
||||
return std::accumulate(dts.begin(), dts.end(), 0_s) / dts.size();
|
||||
}
|
||||
|
||||
units::second_t sysid::GetMeanTimeDelta(const Storage& data) {
|
||||
std::vector<units::second_t> dts;
|
||||
|
||||
for (const auto& pt : data.slowForward) {
|
||||
if (pt.dt > 0_s && pt.dt < 500_ms) {
|
||||
dts.emplace_back(pt.dt);
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto& pt : data.slowBackward) {
|
||||
if (pt.dt > 0_s && pt.dt < 500_ms) {
|
||||
dts.emplace_back(pt.dt);
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto& pt : data.fastForward) {
|
||||
if (pt.dt > 0_s && pt.dt < 500_ms) {
|
||||
dts.emplace_back(pt.dt);
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto& pt : data.fastBackward) {
|
||||
if (pt.dt > 0_s && pt.dt < 500_ms) {
|
||||
dts.emplace_back(pt.dt);
|
||||
}
|
||||
}
|
||||
|
||||
return std::accumulate(dts.begin(), dts.end(), 0_s) / dts.size();
|
||||
}
|
||||
|
||||
void sysid::ApplyMedianFilter(std::vector<PreparedData>* data, int window) {
|
||||
CheckSize(*data, window, "Median Filter");
|
||||
|
||||
frc::MedianFilter<double> medianFilter(window);
|
||||
|
||||
// Load the median filter with the first value for accurate initial behavior
|
||||
for (int i = 0; i < window; i++) {
|
||||
medianFilter.Calculate(data->at(0).velocity);
|
||||
}
|
||||
|
||||
for (size_t i = (window - 1) / 2; i < data->size(); i++) {
|
||||
data->at(i - (window - 1) / 2).velocity =
|
||||
medianFilter.Calculate(data->at(i).velocity);
|
||||
}
|
||||
|
||||
// Run the median filter for the last half window of datapoints by loading the
|
||||
// median filter with the last recorded velocity value
|
||||
for (size_t i = data->size() - (window - 1) / 2; i < data->size(); i++) {
|
||||
data->at(i).velocity =
|
||||
medianFilter.Calculate(data->at(data->size() - 1).velocity);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Removes a substring from a string reference
|
||||
*
|
||||
* @param str The std::string_view that needs modification
|
||||
* @param removeStr The substring that needs to be removed
|
||||
*
|
||||
* @return an std::string without the specified substring
|
||||
*/
|
||||
static std::string RemoveStr(std::string_view str, std::string_view removeStr) {
|
||||
size_t idx = str.find(removeStr);
|
||||
if (idx == std::string_view::npos) {
|
||||
return std::string{str};
|
||||
} else {
|
||||
return fmt::format("{}{}", str.substr(0, idx),
|
||||
str.substr(idx + removeStr.size()));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Figures out the max duration of the Dynamic tests
|
||||
*
|
||||
* @param data The raw data String Map
|
||||
*
|
||||
* @return The maximum duration of the Dynamic Tests
|
||||
*/
|
||||
static units::second_t GetMaxStepTime(
|
||||
wpi::StringMap<std::vector<PreparedData>>& data) {
|
||||
auto maxStepTime = 0_s;
|
||||
for (auto& it : data) {
|
||||
auto& key = it.first;
|
||||
auto& dataset = it.second;
|
||||
|
||||
if (IsRaw(key) && wpi::contains(key, "dynamic")) {
|
||||
if (!dataset.empty()) {
|
||||
auto duration = dataset.back().timestamp - dataset.front().timestamp;
|
||||
if (duration > maxStepTime) {
|
||||
maxStepTime = duration;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return maxStepTime;
|
||||
}
|
||||
|
||||
void sysid::InitialTrimAndFilter(
|
||||
wpi::StringMap<std::vector<PreparedData>>* data,
|
||||
AnalysisManager::Settings* settings,
|
||||
std::vector<units::second_t>& positionDelays,
|
||||
std::vector<units::second_t>& velocityDelays, units::second_t& minStepTime,
|
||||
units::second_t& maxStepTime, std::string_view unit) {
|
||||
auto& preparedData = *data;
|
||||
|
||||
// Find the maximum Step Test Duration of the dynamic tests
|
||||
maxStepTime = GetMaxStepTime(preparedData);
|
||||
|
||||
// Calculate Velocity Threshold if it hasn't been set yet
|
||||
if (settings->velocityThreshold == std::numeric_limits<double>::infinity()) {
|
||||
for (auto& it : preparedData) {
|
||||
auto& key = it.first;
|
||||
auto& dataset = it.second;
|
||||
if (wpi::contains(key, "quasistatic")) {
|
||||
settings->velocityThreshold =
|
||||
std::min(settings->velocityThreshold,
|
||||
GetNoiseFloor(dataset, kNoiseMeanWindow,
|
||||
[](auto&& pt) { return pt.velocity; }));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (auto& it : preparedData) {
|
||||
auto& key = it.first;
|
||||
auto& dataset = it.second;
|
||||
|
||||
// Trim quasistatic test data to remove all points where voltage is zero or
|
||||
// velocity < velocity threshold.
|
||||
if (wpi::contains(key, "quasistatic")) {
|
||||
dataset.erase(std::remove_if(dataset.begin(), dataset.end(),
|
||||
[&](const auto& pt) {
|
||||
return std::abs(pt.voltage) <= 0 ||
|
||||
std::abs(pt.velocity) <
|
||||
settings->velocityThreshold;
|
||||
}),
|
||||
dataset.end());
|
||||
|
||||
// Confirm there's still data
|
||||
if (dataset.empty()) {
|
||||
throw sysid::NoQuasistaticDataError();
|
||||
}
|
||||
}
|
||||
|
||||
// Apply Median filter
|
||||
if (IsFiltered(key) && settings->medianWindow > 1) {
|
||||
ApplyMedianFilter(&dataset, settings->medianWindow);
|
||||
}
|
||||
|
||||
// Recalculate Accel and Cosine
|
||||
PrepareMechData(&dataset, unit);
|
||||
|
||||
// Trims filtered Dynamic Test Data
|
||||
if (IsFiltered(key) && wpi::contains(key, "dynamic")) {
|
||||
// Get the filtered dataset name
|
||||
auto filteredKey = RemoveStr(key, "raw-");
|
||||
|
||||
// Trim Filtered Data
|
||||
auto [tempMinStepTime, positionDelay, velocityDelay] =
|
||||
TrimStepVoltageData(&preparedData[filteredKey], settings, minStepTime,
|
||||
maxStepTime);
|
||||
|
||||
positionDelays.emplace_back(positionDelay);
|
||||
velocityDelays.emplace_back(velocityDelay);
|
||||
|
||||
// Set the Raw Data to start at the same time as the Filtered Data
|
||||
auto startTime = preparedData[filteredKey].front().timestamp;
|
||||
auto rawStart =
|
||||
std::find_if(preparedData[key].begin(), preparedData[key].end(),
|
||||
[&](auto&& pt) { return pt.timestamp == startTime; });
|
||||
preparedData[key].erase(preparedData[key].begin(), rawStart);
|
||||
|
||||
// Confirm there's still data
|
||||
if (preparedData[key].empty()) {
|
||||
throw sysid::NoDynamicDataError();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void sysid::AccelFilter(wpi::StringMap<std::vector<PreparedData>>* data) {
|
||||
auto& preparedData = *data;
|
||||
|
||||
// Remove points with acceleration = 0
|
||||
for (auto& it : preparedData) {
|
||||
auto& dataset = it.second;
|
||||
|
||||
for (size_t i = 0; i < dataset.size(); i++) {
|
||||
if (dataset.at(i).acceleration == 0.0) {
|
||||
dataset.erase(dataset.begin() + i);
|
||||
i--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Confirm there's still data
|
||||
if (std::any_of(preparedData.begin(), preparedData.end(),
|
||||
[](const auto& it) { return it.second.empty(); })) {
|
||||
throw sysid::InvalidDataError(
|
||||
"Acceleration filtering has removed all data.");
|
||||
}
|
||||
}
|
||||
88
tools/sysid/src/main/native/cpp/analysis/OLS.cpp
Normal file
88
tools/sysid/src/main/native/cpp/analysis/OLS.cpp
Normal file
@@ -0,0 +1,88 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/analysis/OLS.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
|
||||
#include <Eigen/Cholesky>
|
||||
|
||||
namespace sysid {
|
||||
|
||||
OLSResult OLS(const Eigen::MatrixXd& X, const Eigen::VectorXd& y) {
|
||||
assert(X.rows() == y.rows());
|
||||
|
||||
// The linear regression model can be written as follows:
|
||||
//
|
||||
// y = Xβ + ε
|
||||
//
|
||||
// where y is the dependent observed variable, X is the matrix of independent
|
||||
// variables, β is a vector of coefficients, and ε is a vector of residuals.
|
||||
//
|
||||
// We want to find the value of β that minimizes εᵀε.
|
||||
//
|
||||
// ε = y − Xβ
|
||||
// εᵀε = (y − Xβ)ᵀ(y − Xβ)
|
||||
//
|
||||
// β̂ = argmin (y − Xβ)ᵀ(y − Xβ)
|
||||
// β
|
||||
//
|
||||
// Take the partial derivative of the cost function with respect to β and set
|
||||
// it equal to zero, then solve for β̂ .
|
||||
//
|
||||
// 0 = −2Xᵀ(y − Xβ̂)
|
||||
// 0 = Xᵀ(y − Xβ̂)
|
||||
// 0 = Xᵀy − XᵀXβ̂
|
||||
// XᵀXβ̂ = Xᵀy
|
||||
// β̂ = (XᵀX)⁻¹Xᵀy
|
||||
|
||||
// β = (XᵀX)⁻¹Xᵀy
|
||||
//
|
||||
// XᵀX is guaranteed to be symmetric positive definite, so an LLT
|
||||
// decomposition can be used.
|
||||
Eigen::MatrixXd β = (X.transpose() * X).llt().solve(X.transpose() * y);
|
||||
|
||||
// Error sum of squares
|
||||
double SSE = (y - X * β).squaredNorm();
|
||||
|
||||
// Sample size
|
||||
int n = X.rows();
|
||||
|
||||
// Number of explanatory variables
|
||||
int p = β.rows();
|
||||
|
||||
// Total sum of squares (total variation in y)
|
||||
//
|
||||
// From slide 24 of
|
||||
// http://www.stat.columbia.edu/~fwood/Teaching/w4315/Fall2009/lecture_11:
|
||||
//
|
||||
// SSTO = yᵀy - 1/n yᵀJy
|
||||
//
|
||||
// where J is a matrix of ones.
|
||||
double SSTO =
|
||||
(y.transpose() * y - 1.0 / y.rows() * y.transpose() *
|
||||
Eigen::MatrixXd::Ones(y.rows(), y.rows()) * y)
|
||||
.value();
|
||||
|
||||
// R² or the coefficient of determination, which represents how much of the
|
||||
// total variation (variation in y) can be explained by the regression model
|
||||
double rSquared = 1.0 - SSE / SSTO;
|
||||
|
||||
// Adjusted R²
|
||||
//
|
||||
// n − 1
|
||||
// R̅² = 1 − (1 − R²) ---------
|
||||
// n − p − 1
|
||||
//
|
||||
// See https://en.wikipedia.org/wiki/Coefficient_of_determination#Adjusted_R2
|
||||
double adjRSquared = 1.0 - (1.0 - rSquared) * ((n - 1.0) / (n - p - 1.0));
|
||||
|
||||
// Root-mean-square error
|
||||
double RMSE = std::sqrt(SSE / n);
|
||||
|
||||
return {{β.data(), β.data() + β.size()}, adjRSquared, RMSE};
|
||||
}
|
||||
|
||||
} // namespace sysid
|
||||
47
tools/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp
Normal file
47
tools/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp
Normal file
@@ -0,0 +1,47 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/analysis/SimpleMotorSim.h"
|
||||
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
#include <frc/system/Discretization.h>
|
||||
#include <wpi/MathExtras.h>
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
SimpleMotorSim::SimpleMotorSim(double Ks, double Kv, double Ka,
|
||||
double initialPosition, double initialVelocity)
|
||||
// dx/dt = Ax + Bu + c sgn(x)
|
||||
: m_A{{0.0, 1.0}, {0.0, -Kv / Ka}}, m_B{0.0, 1.0 / Ka}, m_c{0.0, -Ks / Ka} {
|
||||
Reset(initialPosition, initialVelocity);
|
||||
}
|
||||
|
||||
void SimpleMotorSim::Update(units::volt_t voltage, units::second_t dt) {
|
||||
Eigen::Vector<double, 1> u{voltage.value()};
|
||||
|
||||
// Given dx/dt = Ax + Bu + c sgn(x),
|
||||
// x_k+1 = e^(AT) x_k + A^-1 (e^(AT) - 1) (Bu + c sgn(x))
|
||||
Eigen::Matrix<double, 2, 2> Ad;
|
||||
Eigen::Matrix<double, 2, 1> Bd;
|
||||
frc::DiscretizeAB<2, 1>(m_A, m_B, dt, &Ad, &Bd);
|
||||
m_x = Ad * m_x + Bd * u +
|
||||
Bd * m_B.householderQr().solve(m_c * wpi::sgn(GetVelocity()));
|
||||
}
|
||||
|
||||
double SimpleMotorSim::GetPosition() const {
|
||||
return m_x(0);
|
||||
}
|
||||
|
||||
double SimpleMotorSim::GetVelocity() const {
|
||||
return m_x(1);
|
||||
}
|
||||
|
||||
double SimpleMotorSim::GetAcceleration(units::volt_t voltage) const {
|
||||
Eigen::Vector<double, 1> u{voltage.value()};
|
||||
return (m_A * m_x + m_B * u + m_c * wpi::sgn(GetVelocity()))(1);
|
||||
}
|
||||
|
||||
void SimpleMotorSim::Reset(double position, double velocity) {
|
||||
m_x = Eigen::Vector<double, 2>{position, velocity};
|
||||
}
|
||||
759
tools/sysid/src/main/native/cpp/view/Analyzer.cpp
Normal file
759
tools/sysid/src/main/native/cpp/view/Analyzer.cpp
Normal file
@@ -0,0 +1,759 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/view/Analyzer.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <exception>
|
||||
#include <memory>
|
||||
#include <numbers>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <glass/Context.h>
|
||||
#include <glass/Storage.h>
|
||||
#include <imgui.h>
|
||||
#include <imgui_internal.h>
|
||||
#include <imgui_stdlib.h>
|
||||
#include <wpi/json.h>
|
||||
|
||||
#include "sysid/Util.h"
|
||||
#include "sysid/analysis/AnalysisManager.h"
|
||||
#include "sysid/analysis/AnalysisType.h"
|
||||
#include "sysid/analysis/FeedbackControllerPreset.h"
|
||||
#include "sysid/analysis/FilteringUtils.h"
|
||||
#include "sysid/view/UILayout.h"
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
Analyzer::Analyzer(glass::Storage& storage, wpi::Logger& logger)
|
||||
: m_logger(logger) {
|
||||
// Fill the StringMap with preset values.
|
||||
m_presets["Default"] = presets::kDefault;
|
||||
m_presets["WPILib"] = presets::kWPILib;
|
||||
m_presets["CTRE Phoenix 5"] = presets::kCTREv5;
|
||||
m_presets["CTRE Phoenix 6"] = presets::kCTREv6;
|
||||
m_presets["REV Brushless Encoder Port"] = presets::kREVNEOBuiltIn;
|
||||
m_presets["REV Brushed Encoder Port"] = presets::kREVNonNEO;
|
||||
m_presets["REV Data Port"] = presets::kREVNonNEO;
|
||||
m_presets["Venom"] = presets::kVenom;
|
||||
|
||||
ResetData();
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
|
||||
void Analyzer::UpdateFeedforwardGains() {
|
||||
WPI_INFO(m_logger, "{}", "Gain calc");
|
||||
try {
|
||||
const auto& feedforwardGains = m_manager->CalculateFeedforward();
|
||||
m_feedforwardGains = feedforwardGains;
|
||||
m_accelRSquared = feedforwardGains.olsResult.rSquared;
|
||||
m_accelRMSE = feedforwardGains.olsResult.rmse;
|
||||
m_settings.preset.measurementDelay =
|
||||
m_settings.type == FeedbackControllerLoopType::kPosition
|
||||
// Clamp feedback measurement delay to ≥ 0
|
||||
? units::math::max(0_s, m_manager->GetPositionDelay())
|
||||
: units::math::max(0_s, m_manager->GetVelocityDelay());
|
||||
PrepareGraphs();
|
||||
} catch (const sysid::InvalidDataError& e) {
|
||||
m_state = AnalyzerState::kGeneralDataError;
|
||||
HandleError(e.what());
|
||||
} catch (const sysid::NoQuasistaticDataError& e) {
|
||||
m_state = AnalyzerState::kVelocityThresholdError;
|
||||
HandleError(e.what());
|
||||
} catch (const sysid::NoDynamicDataError& e) {
|
||||
m_state = AnalyzerState::kTestDurationError;
|
||||
HandleError(e.what());
|
||||
} catch (const AnalysisManager::FileReadingError& e) {
|
||||
m_state = AnalyzerState::kFileError;
|
||||
HandleError(e.what());
|
||||
} catch (const wpi::json::exception& e) {
|
||||
m_state = AnalyzerState::kFileError;
|
||||
HandleError(e.what());
|
||||
} catch (const std::exception& e) {
|
||||
m_state = AnalyzerState::kFileError;
|
||||
HandleError(e.what());
|
||||
}
|
||||
}
|
||||
|
||||
void Analyzer::UpdateFeedbackGains() {
|
||||
WPI_INFO(m_logger, "{}", "Updating feedback gains");
|
||||
|
||||
const auto& Kv = m_feedforwardGains.Kv;
|
||||
const auto& Ka = m_feedforwardGains.Ka;
|
||||
if (Kv.isValidGain && Ka.isValidGain) {
|
||||
const auto& fb = m_manager->CalculateFeedback(Kv, Ka);
|
||||
m_timescale = units::second_t{Ka.gain / Kv.gain};
|
||||
m_timescaleValid = true;
|
||||
m_Kp = fb.Kp;
|
||||
m_Kd = fb.Kd;
|
||||
} else {
|
||||
m_timescaleValid = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Analyzer::DisplayDouble(const char* text, double* data,
|
||||
bool readOnly = true) {
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
|
||||
if (readOnly) {
|
||||
return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G",
|
||||
ImGuiInputTextFlags_ReadOnly);
|
||||
} else {
|
||||
return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G");
|
||||
}
|
||||
}
|
||||
|
||||
static void SetPosition(double beginX, double beginY, double xShift,
|
||||
double yShift) {
|
||||
ImGui::SetCursorPos(ImVec2(beginX + xShift * 10 * ImGui::GetFontSize(),
|
||||
beginY + yShift * 1.75 * ImGui::GetFontSize()));
|
||||
}
|
||||
|
||||
bool Analyzer::IsErrorState() {
|
||||
return m_state == AnalyzerState::kVelocityThresholdError ||
|
||||
m_state == AnalyzerState::kTestDurationError ||
|
||||
m_state == AnalyzerState::kGeneralDataError ||
|
||||
m_state == AnalyzerState::kFileError;
|
||||
}
|
||||
|
||||
bool Analyzer::IsDataErrorState() {
|
||||
return m_state == AnalyzerState::kVelocityThresholdError ||
|
||||
m_state == AnalyzerState::kTestDurationError ||
|
||||
m_state == AnalyzerState::kGeneralDataError;
|
||||
}
|
||||
|
||||
void Analyzer::ResetData() {
|
||||
m_plot.ResetData();
|
||||
m_manager = std::make_unique<AnalysisManager>(m_settings, m_logger);
|
||||
m_feedforwardGains = AnalysisManager::FeedforwardGains{};
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
|
||||
bool Analyzer::DisplayResetAndUnitOverride() {
|
||||
auto type = m_manager->GetAnalysisType();
|
||||
auto unit = m_manager->GetUnit();
|
||||
|
||||
float width = ImGui::GetContentRegionAvail().x;
|
||||
ImGui::SameLine(width - ImGui::CalcTextSize("Reset").x);
|
||||
if (ImGui::Button("Reset")) {
|
||||
ResetData();
|
||||
m_state = AnalyzerState::kWaitingForData;
|
||||
return true;
|
||||
}
|
||||
|
||||
ImGui::Spacing();
|
||||
ImGui::Text(
|
||||
"Units: %s\n"
|
||||
"Type: %s",
|
||||
std::string(unit).c_str(), type.name);
|
||||
|
||||
if (ImGui::Button("Override Units")) {
|
||||
ImGui::OpenPopup("Override Units");
|
||||
}
|
||||
|
||||
auto size = ImGui::GetIO().DisplaySize;
|
||||
ImGui::SetNextWindowSize(ImVec2(size.x / 4, size.y * 0.2));
|
||||
if (ImGui::BeginPopupModal("Override Units")) {
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
|
||||
ImGui::Combo("Units", &m_selectedOverrideUnit, kUnits,
|
||||
IM_ARRAYSIZE(kUnits));
|
||||
unit = kUnits[m_selectedOverrideUnit];
|
||||
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
|
||||
|
||||
if (ImGui::Button("Close")) {
|
||||
ImGui::CloseCurrentPopup();
|
||||
m_manager->OverrideUnits(unit);
|
||||
PrepareData();
|
||||
}
|
||||
|
||||
ImGui::EndPopup();
|
||||
}
|
||||
|
||||
ImGui::SameLine();
|
||||
if (ImGui::Button("Reset Units from JSON")) {
|
||||
m_manager->ResetUnitsFromJSON();
|
||||
PrepareData();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void Analyzer::ConfigParamsOnFileSelect() {
|
||||
WPI_INFO(m_logger, "{}", "Configuring Params");
|
||||
m_stepTestDuration = m_settings.stepTestDuration.to<float>();
|
||||
|
||||
// Estimate qp as 1/10 native distance unit
|
||||
m_settings.lqr.qp = 0.1;
|
||||
// Estimate qv as 1/4 * max velocity = 1/4 * (12V - kS) / kV
|
||||
m_settings.lqr.qv =
|
||||
0.25 * (12.0 - m_feedforwardGains.Ks.gain) / m_feedforwardGains.Kv.gain;
|
||||
}
|
||||
|
||||
void Analyzer::Display() {
|
||||
DisplayGraphs();
|
||||
|
||||
switch (m_state) {
|
||||
case AnalyzerState::kWaitingForData: {
|
||||
ImGui::Text(
|
||||
"SysId is currently in theoretical analysis mode.\n"
|
||||
"To analyze recorded test data, select a "
|
||||
"data file (.wpilog).");
|
||||
sysid::CreateTooltip(
|
||||
"Theoretical feedback gains can be calculated from a "
|
||||
"physical model of the mechanism being controlled. "
|
||||
"Theoretical gains for several common mechanisms can "
|
||||
"be obtained from ReCalc (https://reca.lc).");
|
||||
ImGui::Spacing();
|
||||
ImGui::Spacing();
|
||||
|
||||
ImGui::SetNextItemOpen(true, ImGuiCond_Once);
|
||||
if (ImGui::CollapsingHeader("Feedforward Gains (Theoretical)")) {
|
||||
float beginX = ImGui::GetCursorPosX();
|
||||
float beginY = ImGui::GetCursorPosY();
|
||||
CollectFeedforwardGains(beginX, beginY);
|
||||
}
|
||||
ImGui::SetNextItemOpen(true, ImGuiCond_Once);
|
||||
if (ImGui::CollapsingHeader("Feedback Analysis")) {
|
||||
DisplayFeedbackGains();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case AnalyzerState::kNominalDisplay: { // Allow the user to select which
|
||||
// data set they want analyzed and
|
||||
// add a
|
||||
// reset button. Also show the units and the units per rotation.
|
||||
if (DisplayResetAndUnitOverride()) {
|
||||
return;
|
||||
}
|
||||
ImGui::Spacing();
|
||||
ImGui::Spacing();
|
||||
|
||||
ImGui::SetNextItemOpen(true, ImGuiCond_Once);
|
||||
if (ImGui::CollapsingHeader("Feedforward Analysis")) {
|
||||
float beginX = ImGui::GetCursorPosX();
|
||||
float beginY = ImGui::GetCursorPosY();
|
||||
DisplayFeedforwardGains(beginX, beginY);
|
||||
}
|
||||
ImGui::SetNextItemOpen(true, ImGuiCond_Once);
|
||||
if (ImGui::CollapsingHeader("Feedback Analysis")) {
|
||||
DisplayFeedbackGains();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case AnalyzerState::kFileError: {
|
||||
CreateErrorPopup(m_errorPopup, m_exception);
|
||||
if (!m_errorPopup) {
|
||||
m_state = AnalyzerState::kWaitingForData;
|
||||
return;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case AnalyzerState::kMissingTestsError: {
|
||||
CreateErrorPopup(m_errorPopup, m_exception);
|
||||
if (!m_errorPopup) {
|
||||
m_state = AnalyzerState::kWaitingForData;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case AnalyzerState::kGeneralDataError:
|
||||
case AnalyzerState::kTestDurationError:
|
||||
case AnalyzerState::kVelocityThresholdError: {
|
||||
CreateErrorPopup(m_errorPopup, m_exception);
|
||||
if (DisplayResetAndUnitOverride()) {
|
||||
return;
|
||||
}
|
||||
float beginX = ImGui::GetCursorPosX();
|
||||
float beginY = ImGui::GetCursorPosY();
|
||||
DisplayFeedforwardParameters(beginX, beginY);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Analyzer::PrepareData() {
|
||||
WPI_INFO(m_logger, "{}", "Preparing data");
|
||||
try {
|
||||
if (m_missingTests.size() > 0) {
|
||||
throw sysid::MissingTestsError{m_missingTests};
|
||||
}
|
||||
m_manager->PrepareData();
|
||||
UpdateFeedforwardGains();
|
||||
UpdateFeedbackGains();
|
||||
} catch (const sysid::InvalidDataError& e) {
|
||||
m_state = AnalyzerState::kGeneralDataError;
|
||||
HandleError(e.what());
|
||||
} catch (const sysid::NoQuasistaticDataError& e) {
|
||||
m_state = AnalyzerState::kVelocityThresholdError;
|
||||
HandleError(e.what());
|
||||
} catch (const sysid::NoDynamicDataError& e) {
|
||||
m_state = AnalyzerState::kTestDurationError;
|
||||
HandleError(e.what());
|
||||
} catch (const sysid::MissingTestsError& e) {
|
||||
m_state = AnalyzerState::kMissingTestsError;
|
||||
HandleError(e.what());
|
||||
} catch (const AnalysisManager::FileReadingError& e) {
|
||||
m_state = AnalyzerState::kFileError;
|
||||
HandleError(e.what());
|
||||
} catch (const wpi::json::exception& e) {
|
||||
m_state = AnalyzerState::kFileError;
|
||||
HandleError(e.what());
|
||||
} catch (const std::exception& e) {
|
||||
m_state = AnalyzerState::kFileError;
|
||||
HandleError(e.what());
|
||||
}
|
||||
}
|
||||
|
||||
void Analyzer::PrepareRawGraphs() {
|
||||
if (m_manager->HasData()) {
|
||||
AbortDataPrep();
|
||||
m_dataThread = std::thread([&] {
|
||||
m_plot.SetRawData(m_manager->GetOriginalData(), m_manager->GetUnit(),
|
||||
m_abortDataPrep);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
void Analyzer::PrepareGraphs() {
|
||||
if (m_manager->HasData()) {
|
||||
WPI_INFO(m_logger, "{}", "Graph state");
|
||||
AbortDataPrep();
|
||||
m_dataThread = std::thread([&] {
|
||||
m_plot.SetData(m_manager->GetRawData(), m_manager->GetFilteredData(),
|
||||
m_manager->GetUnit(), m_feedforwardGains,
|
||||
m_manager->GetStartTimes(), m_manager->GetAnalysisType(),
|
||||
m_abortDataPrep);
|
||||
});
|
||||
UpdateFeedbackGains();
|
||||
m_state = AnalyzerState::kNominalDisplay;
|
||||
}
|
||||
}
|
||||
|
||||
void Analyzer::HandleError(std::string_view msg) {
|
||||
m_exception = msg;
|
||||
m_errorPopup = true;
|
||||
PrepareRawGraphs();
|
||||
}
|
||||
|
||||
void Analyzer::SetMissingTests(const std::vector<std::string>& missingTests) {
|
||||
m_missingTests = missingTests;
|
||||
}
|
||||
|
||||
void Analyzer::DisplayGraphs() {
|
||||
ImGui::SetNextWindowPos(ImVec2{kDiagnosticPlotWindowPos},
|
||||
ImGuiCond_FirstUseEver);
|
||||
ImGui::SetNextWindowSize(ImVec2{kDiagnosticPlotWindowSize},
|
||||
ImGuiCond_FirstUseEver);
|
||||
ImGui::Begin("Diagnostic Plots");
|
||||
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
|
||||
if (ImGui::SliderFloat("Point Size", &m_plot.m_pointSize, 1, 2, "%.2f")) {
|
||||
if (!IsErrorState()) {
|
||||
PrepareGraphs();
|
||||
} else {
|
||||
PrepareRawGraphs();
|
||||
}
|
||||
}
|
||||
|
||||
ImGui::SameLine();
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
|
||||
const char* items[] = {"Forward", "Backward"};
|
||||
if (ImGui::Combo("Direction", &m_plot.m_direction, items, 2)) {
|
||||
if (!IsErrorState()) {
|
||||
PrepareGraphs();
|
||||
} else {
|
||||
PrepareRawGraphs();
|
||||
}
|
||||
}
|
||||
|
||||
// If the plots were already loaded, store the scroll position. Else go to
|
||||
// the last recorded scroll position if they have just been initialized
|
||||
bool plotsLoaded = m_plot.DisplayPlots();
|
||||
if (plotsLoaded) {
|
||||
if (m_prevPlotsLoaded) {
|
||||
m_graphScroll = ImGui::GetScrollY();
|
||||
} else {
|
||||
ImGui::SetScrollY(m_graphScroll);
|
||||
}
|
||||
|
||||
// If a JSON is selected
|
||||
if (m_state == AnalyzerState::kNominalDisplay) {
|
||||
DisplayDouble("Acceleration R²", &m_accelRSquared);
|
||||
CreateTooltip(
|
||||
"The coefficient of determination of the OLS fit of acceleration "
|
||||
"versus velocity and voltage. Acceleration is extremely noisy, "
|
||||
"so this is generally quite small.");
|
||||
|
||||
ImGui::SameLine();
|
||||
DisplayDouble("Acceleration RMSE", &m_accelRMSE);
|
||||
CreateTooltip(
|
||||
"The standard deviation of the residuals from the predicted "
|
||||
"acceleration."
|
||||
"This can be interpreted loosely as the mean measured disturbance "
|
||||
"from the \"ideal\" system equation.");
|
||||
|
||||
DisplayDouble("Sim velocity R²", m_plot.GetSimRSquared());
|
||||
CreateTooltip(
|
||||
"The coefficient of determination the simulated velocity. "
|
||||
"Velocity is much less-noisy than acceleration, so this "
|
||||
"is pretty close to 1 for a decent fit.");
|
||||
|
||||
ImGui::SameLine();
|
||||
DisplayDouble("Sim velocity RMSE", m_plot.GetSimRMSE());
|
||||
CreateTooltip(
|
||||
"The standard deviation of the residuals from the simulated velocity "
|
||||
"predictions - essentially the size of the mean error of the "
|
||||
"simulated model "
|
||||
"in the recorded velocity units.");
|
||||
}
|
||||
}
|
||||
m_prevPlotsLoaded = plotsLoaded;
|
||||
|
||||
ImGui::End();
|
||||
}
|
||||
|
||||
void Analyzer::AnalyzeData() {
|
||||
m_manager = std::make_unique<AnalysisManager>(m_data, m_settings, m_logger);
|
||||
PrepareData();
|
||||
m_dataset = 0;
|
||||
ConfigParamsOnFileSelect();
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
|
||||
void Analyzer::AbortDataPrep() {
|
||||
if (m_dataThread.joinable()) {
|
||||
m_abortDataPrep = true;
|
||||
m_dataThread.join();
|
||||
m_abortDataPrep = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Analyzer::DisplayFeedforwardParameters(float beginX, float beginY) {
|
||||
// Increase spacing to not run into trackwidth in the normal analyzer view
|
||||
constexpr double kHorizontalOffset = 1.1;
|
||||
SetPosition(beginX, beginY, kHorizontalOffset, 0);
|
||||
|
||||
bool displayAll =
|
||||
!IsErrorState() || m_state == AnalyzerState::kGeneralDataError;
|
||||
|
||||
if (displayAll) {
|
||||
// Wait for enter before refresh so double digit entries like "15" don't
|
||||
// prematurely refresh with "1". That can cause display stuttering.
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
int window = m_settings.medianWindow;
|
||||
if (ImGui::InputInt("Window Size", &window, 0, 0,
|
||||
ImGuiInputTextFlags_EnterReturnsTrue)) {
|
||||
m_settings.medianWindow = std::clamp(window, 1, 15);
|
||||
PrepareData();
|
||||
}
|
||||
|
||||
CreateTooltip(
|
||||
"The number of samples in the velocity median "
|
||||
"filter's sliding window.");
|
||||
}
|
||||
|
||||
if (displayAll || m_state == AnalyzerState::kVelocityThresholdError) {
|
||||
// Wait for enter before refresh so decimal inputs like "0.2" don't
|
||||
// prematurely refresh with a velocity threshold of "0".
|
||||
SetPosition(beginX, beginY, kHorizontalOffset, 1);
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
double threshold = m_settings.velocityThreshold;
|
||||
if (ImGui::InputDouble("Velocity Threshold", &threshold, 0.0, 0.0, "%.3f",
|
||||
ImGuiInputTextFlags_EnterReturnsTrue)) {
|
||||
m_settings.velocityThreshold = std::max(0.0, threshold);
|
||||
PrepareData();
|
||||
}
|
||||
CreateTooltip("Velocity data below this threshold will be ignored.");
|
||||
}
|
||||
|
||||
if (displayAll || m_state == AnalyzerState::kTestDurationError) {
|
||||
SetPosition(beginX, beginY, kHorizontalOffset, 2);
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
if (ImGui::SliderFloat("Test Duration", &m_stepTestDuration,
|
||||
m_manager->GetMinStepTime().value(),
|
||||
m_manager->GetMaxStepTime().value(), "%.2f")) {
|
||||
m_settings.stepTestDuration = units::second_t{m_stepTestDuration};
|
||||
PrepareData();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Analyzer::CollectFeedforwardGains(float beginX, float beginY) {
|
||||
SetPosition(beginX, beginY, 0, 0);
|
||||
if (DisplayDouble("Kv", &m_feedforwardGains.Kv.gain, false)) {
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
|
||||
SetPosition(beginX, beginY, 0, 1);
|
||||
if (DisplayDouble("Ka", &m_feedforwardGains.Ka.gain, false)) {
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
|
||||
SetPosition(beginX, beginY, 0, 2);
|
||||
// Show Timescale
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
DisplayDouble("Response Timescale (ms)",
|
||||
reinterpret_cast<double*>(&m_timescale));
|
||||
CreateTooltip(
|
||||
"The characteristic timescale of the system response in milliseconds. "
|
||||
"Both the control loop period and total signal delay should be "
|
||||
"at least 3-5 times shorter than this to optimally control the "
|
||||
"system.");
|
||||
}
|
||||
|
||||
void Analyzer::DisplayFeedforwardGain(const char* text,
|
||||
AnalysisManager::FeedforwardGain& ffGain,
|
||||
bool readOnly = true) {
|
||||
DisplayDouble(text, &ffGain.gain, readOnly);
|
||||
if (!ffGain.isValidGain) {
|
||||
// Display invalid gain message with warning and tooltip
|
||||
CreateErrorTooltip(ffGain.errorMessage.c_str());
|
||||
}
|
||||
|
||||
// Display descriptor message as tooltip, whether the gain is valid or not
|
||||
CreateTooltip(ffGain.descriptor.c_str());
|
||||
}
|
||||
|
||||
void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) {
|
||||
SetPosition(beginX, beginY, 0, 0);
|
||||
DisplayFeedforwardGain("Ks", m_feedforwardGains.Ks);
|
||||
|
||||
SetPosition(beginX, beginY, 0, 1);
|
||||
DisplayFeedforwardGain("Kv", m_feedforwardGains.Kv);
|
||||
|
||||
SetPosition(beginX, beginY, 0, 2);
|
||||
DisplayFeedforwardGain("Ka", m_feedforwardGains.Ka);
|
||||
|
||||
SetPosition(beginX, beginY, 0, 3);
|
||||
// Show Timescale
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
DisplayDouble("Response Timescale (ms)",
|
||||
reinterpret_cast<double*>(&m_timescale));
|
||||
if (!m_timescaleValid) {
|
||||
CreateErrorTooltip(
|
||||
"Response timescale calculation invalid. Ensure that calculated gains "
|
||||
"are valid.");
|
||||
}
|
||||
CreateTooltip(
|
||||
"The characteristic timescale of the system response in milliseconds. "
|
||||
"Both the control loop period and total signal delay should be "
|
||||
"at least 3-5 times shorter than this to optimally control the "
|
||||
"system.");
|
||||
|
||||
SetPosition(beginX, beginY, 0, 4);
|
||||
auto positionDelay = m_manager->GetPositionDelay();
|
||||
DisplayDouble("Position Measurement Delay (ms)",
|
||||
reinterpret_cast<double*>(&positionDelay));
|
||||
CreateTooltip(
|
||||
"The average elapsed time between the first application of "
|
||||
"voltage and the first detected change in mechanism position "
|
||||
"in the step-voltage tests. This includes CAN delays, and "
|
||||
"may overestimate the true delay for on-motor-controller "
|
||||
"feedback loops by up to 20ms.");
|
||||
|
||||
SetPosition(beginX, beginY, 0, 5);
|
||||
auto velocityDelay = m_manager->GetVelocityDelay();
|
||||
DisplayDouble("Velocity Measurement Delay (ms)",
|
||||
reinterpret_cast<double*>(&velocityDelay));
|
||||
CreateTooltip(
|
||||
"The average elapsed time between the first application of "
|
||||
"voltage and the maximum calculated mechanism acceleration "
|
||||
"in the step-voltage tests. This includes CAN delays, and "
|
||||
"may overestimate the true delay for on-motor-controller "
|
||||
"feedback loops by up to 20ms.");
|
||||
|
||||
SetPosition(beginX, beginY, 0, 6);
|
||||
|
||||
if (m_manager->GetAnalysisType() == analysis::kElevator) {
|
||||
DisplayFeedforwardGain("Kg", m_feedforwardGains.Kg);
|
||||
} else if (m_manager->GetAnalysisType() == analysis::kArm) {
|
||||
DisplayFeedforwardGain("Kg", m_feedforwardGains.Kg);
|
||||
|
||||
double offset;
|
||||
auto unit = m_manager->GetUnit();
|
||||
if (unit == "Radians") {
|
||||
offset = m_feedforwardGains.offset.gain;
|
||||
} else if (unit == "Degrees") {
|
||||
offset = m_feedforwardGains.offset.gain / std::numbers::pi * 180.0;
|
||||
} else if (unit == "Rotations") {
|
||||
offset = m_feedforwardGains.offset.gain / (2 * std::numbers::pi);
|
||||
}
|
||||
DisplayDouble(
|
||||
fmt::format("Angle offset to horizontal ({})", GetAbbreviation(unit))
|
||||
.c_str(),
|
||||
&offset);
|
||||
CreateTooltip(
|
||||
"This is the angle offset which, when added to the angle measurement, "
|
||||
"zeroes it out when the arm is horizontal. This is needed for the arm "
|
||||
"feedforward to work.");
|
||||
}
|
||||
double endY = ImGui::GetCursorPosY();
|
||||
|
||||
DisplayFeedforwardParameters(beginX, beginY);
|
||||
ImGui::SetCursorPosY(endY);
|
||||
}
|
||||
|
||||
void Analyzer::DisplayFeedbackGains() {
|
||||
// Allow the user to select a feedback controller preset.
|
||||
ImGui::Spacing();
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
|
||||
if (ImGui::Combo("Gain Preset", &m_selectedPreset, kPresetNames,
|
||||
IM_ARRAYSIZE(kPresetNames))) {
|
||||
m_settings.preset = m_presets[kPresetNames[m_selectedPreset]];
|
||||
m_settings.type = FeedbackControllerLoopType::kVelocity;
|
||||
m_selectedLoopType =
|
||||
static_cast<int>(FeedbackControllerLoopType::kVelocity);
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
ImGui::SameLine();
|
||||
sysid::CreateTooltip(
|
||||
"Gain presets represent how feedback gains are calculated for your "
|
||||
"specific feedback controller:\n\n"
|
||||
"Default, WPILib (2020-): For use with the new WPILib PIDController "
|
||||
"class.\n"
|
||||
"WPILib (Pre-2020): For use with the old WPILib PIDController class.\n"
|
||||
"CTRE: For use with CTRE units. These are the default units that ship "
|
||||
"with CTRE motor controllers.\n"
|
||||
"REV (Brushless): For use with NEO and NEO 550 motors on a SPARK MAX.\n"
|
||||
"REV (Brushed): For use with brushed motors connected to a SPARK MAX.");
|
||||
|
||||
if (m_settings.preset != m_presets[kPresetNames[m_selectedPreset]]) {
|
||||
ImGui::SameLine();
|
||||
ImGui::TextDisabled("(modified)");
|
||||
}
|
||||
|
||||
// Show our feedback controller preset values.
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
double value = m_settings.preset.outputConversionFactor * 12;
|
||||
if (ImGui::InputDouble("Max Controller Output", &value, 0.0, 0.0, "%.1f") &&
|
||||
value > 0) {
|
||||
m_settings.preset.outputConversionFactor = value / 12.0;
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
value = m_settings.preset.outputVelocityTimeFactor;
|
||||
if (ImGui::InputDouble("Velocity Denominator Units (s)", &value, 0.0, 0.0,
|
||||
"%.1f") &&
|
||||
value > 0) {
|
||||
m_settings.preset.outputVelocityTimeFactor = value;
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
|
||||
sysid::CreateTooltip(
|
||||
"This represents the denominator of the velocity unit used by the "
|
||||
"feedback controller. For example, CTRE uses 100 ms = 0.1 s.");
|
||||
|
||||
auto ShowPresetValue = [](const char* text, double* data,
|
||||
float cursorX = 0.0f) {
|
||||
if (cursorX > 0) {
|
||||
ImGui::SetCursorPosX(cursorX);
|
||||
}
|
||||
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G");
|
||||
};
|
||||
|
||||
// Show controller period.
|
||||
if (ShowPresetValue("Controller Period (ms)",
|
||||
reinterpret_cast<double*>(&m_settings.preset.period))) {
|
||||
if (m_settings.preset.period > 0_s &&
|
||||
m_settings.preset.measurementDelay >= 0_s) {
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
}
|
||||
|
||||
// Show whether the controller gains are time-normalized.
|
||||
if (ImGui::Checkbox("Time-Normalized?", &m_settings.preset.normalized)) {
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
|
||||
// Show position/velocity measurement delay.
|
||||
if (ShowPresetValue(
|
||||
"Measurement Delay (ms)",
|
||||
reinterpret_cast<double*>(&m_settings.preset.measurementDelay))) {
|
||||
if (m_settings.preset.period > 0_s &&
|
||||
m_settings.preset.measurementDelay >= 0_s) {
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
}
|
||||
|
||||
sysid::CreateTooltip(
|
||||
"The average measurement delay of the process variable in milliseconds. "
|
||||
"This may depend on your encoder settings and choice of motor "
|
||||
"controller. Default velocity filtering windows are quite long "
|
||||
"on many motor controllers, so be careful that this value is "
|
||||
"accurate if the characteristic timescale of the mechanism "
|
||||
"is small.");
|
||||
|
||||
ImGui::Separator();
|
||||
ImGui::Spacing();
|
||||
|
||||
// Allow the user to select a loop type.
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple);
|
||||
if (ImGui::Combo("Loop Type", &m_selectedLoopType, kLoopTypes,
|
||||
IM_ARRAYSIZE(kLoopTypes))) {
|
||||
m_settings.type =
|
||||
static_cast<FeedbackControllerLoopType>(m_selectedLoopType);
|
||||
if (m_state == AnalyzerState::kWaitingForData) {
|
||||
m_settings.preset.measurementDelay = 0_ms;
|
||||
} else {
|
||||
if (m_settings.type == FeedbackControllerLoopType::kPosition) {
|
||||
m_settings.preset.measurementDelay = m_manager->GetPositionDelay();
|
||||
} else {
|
||||
m_settings.preset.measurementDelay = m_manager->GetVelocityDelay();
|
||||
}
|
||||
}
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
|
||||
ImGui::Spacing();
|
||||
|
||||
// Show Kp and Kd.
|
||||
float beginY = ImGui::GetCursorPosY();
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
DisplayDouble("Kp", &m_Kp);
|
||||
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
|
||||
DisplayDouble("Kd", &m_Kd);
|
||||
|
||||
// Come back to the starting y pos.
|
||||
ImGui::SetCursorPosY(beginY);
|
||||
|
||||
if (m_selectedLoopType == 0) {
|
||||
std::string unit;
|
||||
if (m_state != AnalyzerState::kWaitingForData) {
|
||||
unit = fmt::format(" ({})", GetAbbreviation(m_manager->GetUnit()));
|
||||
}
|
||||
|
||||
ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
|
||||
if (DisplayDouble(fmt::format("Max Position Error{}", unit).c_str(),
|
||||
&m_settings.lqr.qp, false)) {
|
||||
if (m_settings.lqr.qp > 0) {
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::string unit;
|
||||
if (m_state != AnalyzerState::kWaitingForData) {
|
||||
unit = fmt::format(" ({}/s)", GetAbbreviation(m_manager->GetUnit()));
|
||||
}
|
||||
|
||||
ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
|
||||
if (DisplayDouble(fmt::format("Max Velocity Error{}", unit).c_str(),
|
||||
&m_settings.lqr.qv, false)) {
|
||||
if (m_settings.lqr.qv > 0) {
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
}
|
||||
ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
|
||||
if (DisplayDouble("Max Control Effort (V)", &m_settings.lqr.r, false)) {
|
||||
if (m_settings.lqr.r > 0) {
|
||||
UpdateFeedbackGains();
|
||||
}
|
||||
}
|
||||
}
|
||||
534
tools/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp
Normal file
534
tools/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp
Normal file
@@ -0,0 +1,534 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/view/AnalyzerPlot.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
#include <mutex>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <units/math.h>
|
||||
|
||||
#include "sysid/Util.h"
|
||||
#include "sysid/analysis/AnalysisManager.h"
|
||||
#include "sysid/analysis/ArmSim.h"
|
||||
#include "sysid/analysis/ElevatorSim.h"
|
||||
#include "sysid/analysis/FilteringUtils.h"
|
||||
#include "sysid/analysis/SimpleMotorSim.h"
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
static ImPlotPoint Getter(int idx, void* data) {
|
||||
return static_cast<ImPlotPoint*>(data)[idx];
|
||||
}
|
||||
|
||||
template <typename Model>
|
||||
static std::vector<std::vector<ImPlotPoint>> PopulateTimeDomainSim(
|
||||
const std::vector<PreparedData>& data,
|
||||
const std::array<units::second_t, 4>& startTimes, size_t step, Model model,
|
||||
double* simSquaredErrorSum, double* squaredVariationSum,
|
||||
int* timeSeriesPoints) {
|
||||
// Create the vector of ImPlotPoints that will contain our simulated data.
|
||||
std::vector<std::vector<ImPlotPoint>> pts;
|
||||
std::vector<ImPlotPoint> tmp;
|
||||
|
||||
auto startTime = data[0].timestamp;
|
||||
|
||||
tmp.emplace_back(startTime.value(), data[0].velocity);
|
||||
|
||||
model.Reset(data[0].position, data[0].velocity);
|
||||
units::second_t t = 0_s;
|
||||
|
||||
for (size_t i = 1; i < data.size(); ++i) {
|
||||
const auto& now = data[i];
|
||||
const auto& pre = data[i - 1];
|
||||
|
||||
t += now.timestamp - pre.timestamp;
|
||||
|
||||
// If the current time stamp and previous time stamp are across a test's
|
||||
// start timestamp, it is the start of a new test and the model needs to be
|
||||
// reset.
|
||||
if (std::find(startTimes.begin(), startTimes.end(), now.timestamp) !=
|
||||
startTimes.end()) {
|
||||
pts.emplace_back(std::move(tmp));
|
||||
model.Reset(now.position, now.velocity);
|
||||
continue;
|
||||
}
|
||||
|
||||
model.Update(units::volt_t{pre.voltage}, now.timestamp - pre.timestamp);
|
||||
tmp.emplace_back((startTime + t).value(), model.GetVelocity());
|
||||
*simSquaredErrorSum += std::pow(now.velocity - model.GetVelocity(), 2);
|
||||
*squaredVariationSum += std::pow(now.velocity, 2);
|
||||
++(*timeSeriesPoints);
|
||||
}
|
||||
|
||||
pts.emplace_back(std::move(tmp));
|
||||
return pts;
|
||||
}
|
||||
|
||||
AnalyzerPlot::AnalyzerPlot(wpi::Logger& logger) : m_logger(logger) {}
|
||||
|
||||
void AnalyzerPlot::SetRawTimeData(const std::vector<PreparedData>& rawSlow,
|
||||
const std::vector<PreparedData>& rawFast,
|
||||
std::atomic<bool>& abort) {
|
||||
auto rawSlowStep = std::ceil(rawSlow.size() * 1.0 / kMaxSize * 4);
|
||||
auto rawFastStep = std::ceil(rawFast.size() * 1.0 / kMaxSize * 4);
|
||||
// Populate Raw Slow Time Series Data
|
||||
for (size_t i = 0; i < rawSlow.size(); i += rawSlowStep) {
|
||||
if (abort) {
|
||||
return;
|
||||
}
|
||||
m_quasistaticData.rawData.emplace_back((rawSlow[i].timestamp).value(),
|
||||
rawSlow[i].velocity);
|
||||
}
|
||||
|
||||
// Populate Raw fast Time Series Data
|
||||
for (size_t i = 0; i < rawFast.size(); i += rawFastStep) {
|
||||
if (abort) {
|
||||
return;
|
||||
}
|
||||
m_dynamicData.rawData.emplace_back((rawFast[i].timestamp).value(),
|
||||
rawFast[i].velocity);
|
||||
}
|
||||
}
|
||||
|
||||
void AnalyzerPlot::ResetData() {
|
||||
m_quasistaticData.Clear();
|
||||
m_dynamicData.Clear();
|
||||
m_regressionData.Clear();
|
||||
m_timestepData.Clear();
|
||||
|
||||
FitPlots();
|
||||
}
|
||||
|
||||
void AnalyzerPlot::SetGraphLabels(std::string_view unit) {
|
||||
std::string_view abbreviation = GetAbbreviation(unit);
|
||||
m_velocityLabel = fmt::format("Velocity ({}/s)", abbreviation);
|
||||
m_accelerationLabel = fmt::format("Acceleration ({}/s²)", abbreviation);
|
||||
m_velPortionAccelLabel =
|
||||
fmt::format("Velocity-Portion Accel ({}/s²)", abbreviation);
|
||||
}
|
||||
|
||||
void AnalyzerPlot::SetRawData(const Storage& data, std::string_view unit,
|
||||
std::atomic<bool>& abort) {
|
||||
const auto& [slowForward, slowBackward, fastForward, fastBackward] = data;
|
||||
const auto& slow = m_direction == 0 ? slowForward : slowBackward;
|
||||
const auto& fast = m_direction == 0 ? fastForward : fastBackward;
|
||||
|
||||
SetGraphLabels(unit);
|
||||
|
||||
std::scoped_lock lock(m_mutex);
|
||||
|
||||
ResetData();
|
||||
SetRawTimeData(slow, fast, abort);
|
||||
}
|
||||
|
||||
void AnalyzerPlot::SetData(const Storage& rawData, const Storage& filteredData,
|
||||
std::string_view unit,
|
||||
const AnalysisManager::FeedforwardGains& ffGains,
|
||||
const std::array<units::second_t, 4>& startTimes,
|
||||
AnalysisType type, std::atomic<bool>& abort) {
|
||||
double simSquaredErrorSum = 0;
|
||||
double squaredVariationSum = 0;
|
||||
int timeSeriesPoints = 0;
|
||||
|
||||
const auto& Ks = ffGains.Ks.gain;
|
||||
const auto& Kv = ffGains.Kv.gain;
|
||||
const auto& Ka = ffGains.Ka.gain;
|
||||
|
||||
auto& [slowForward, slowBackward, fastForward, fastBackward] = filteredData;
|
||||
auto& [rawSlowForward, rawSlowBackward, rawFastForward, rawFastBackward] =
|
||||
rawData;
|
||||
|
||||
const auto slow = AnalysisManager::DataConcat(slowForward, slowBackward);
|
||||
const auto fast = AnalysisManager::DataConcat(fastForward, fastBackward);
|
||||
const auto rawSlow =
|
||||
AnalysisManager::DataConcat(rawSlowForward, rawSlowBackward);
|
||||
const auto rawFast =
|
||||
AnalysisManager::DataConcat(rawFastForward, rawFastBackward);
|
||||
|
||||
SetGraphLabels(unit);
|
||||
|
||||
std::scoped_lock lock(m_mutex);
|
||||
|
||||
ResetData();
|
||||
|
||||
// Calculate step sizes to ensure that we only use the memory that we
|
||||
// allocated.
|
||||
auto slowStep = std::ceil(slow.size() * 1.0 / kMaxSize * 4);
|
||||
auto fastStep = std::ceil(fast.size() * 1.0 / kMaxSize * 4);
|
||||
|
||||
units::second_t dtMean = GetMeanTimeDelta(filteredData);
|
||||
|
||||
// Velocity-vs-time plots
|
||||
{
|
||||
const auto& slow = m_direction == 0 ? slowForward : slowBackward;
|
||||
const auto& fast = m_direction == 0 ? fastForward : fastBackward;
|
||||
const auto& rawSlow = m_direction == 0 ? rawSlowForward : rawSlowBackward;
|
||||
const auto& rawFast = m_direction == 0 ? rawFastForward : rawFastBackward;
|
||||
|
||||
// Populate quasistatic time-domain graphs
|
||||
for (size_t i = 0; i < slow.size(); i += slowStep) {
|
||||
if (abort) {
|
||||
return;
|
||||
}
|
||||
|
||||
m_quasistaticData.filteredData.emplace_back((slow[i].timestamp).value(),
|
||||
slow[i].velocity);
|
||||
|
||||
if (i > 0) {
|
||||
// If the current timestamp is not in the startTimes array, it is the
|
||||
// during a test and should be included. If it is in the startTimes
|
||||
// array, it is the beginning of a new test and the dt will be inflated.
|
||||
// Therefore we skip those to exclude that dt and effectively reset dt
|
||||
// calculations.
|
||||
if (slow[i].dt > 0_s &&
|
||||
std::find(startTimes.begin(), startTimes.end(),
|
||||
slow[i].timestamp) == startTimes.end()) {
|
||||
m_timestepData.data.emplace_back(
|
||||
(slow[i].timestamp).value(),
|
||||
units::millisecond_t{slow[i].dt}.value());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Populate dynamic time-domain graphs
|
||||
for (size_t i = 0; i < fast.size(); i += fastStep) {
|
||||
if (abort) {
|
||||
return;
|
||||
}
|
||||
|
||||
m_dynamicData.filteredData.emplace_back((fast[i].timestamp).value(),
|
||||
fast[i].velocity);
|
||||
|
||||
if (i > 0) {
|
||||
// If the current timestamp is not in the startTimes array, it is the
|
||||
// during a test and should be included. If it is in the startTimes
|
||||
// array, it is the beginning of a new test and the dt will be inflated.
|
||||
// Therefore we skip those to exclude that dt and effectively reset dt
|
||||
// calculations.
|
||||
if (fast[i].dt > 0_s &&
|
||||
std::find(startTimes.begin(), startTimes.end(),
|
||||
fast[i].timestamp) == startTimes.end()) {
|
||||
m_timestepData.data.emplace_back(
|
||||
(fast[i].timestamp).value(),
|
||||
units::millisecond_t{fast[i].dt}.value());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
SetRawTimeData(rawSlow, rawFast, abort);
|
||||
|
||||
// Populate simulated time domain data
|
||||
if (type == analysis::kElevator) {
|
||||
const auto& Kg = ffGains.Kg.gain;
|
||||
m_quasistaticData.simData = PopulateTimeDomainSim(
|
||||
rawSlow, startTimes, fastStep, sysid::ElevatorSim{Ks, Kv, Ka, Kg},
|
||||
&simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
|
||||
m_dynamicData.simData = PopulateTimeDomainSim(
|
||||
rawFast, startTimes, fastStep, sysid::ElevatorSim{Ks, Kv, Ka, Kg},
|
||||
&simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
|
||||
} else if (type == analysis::kArm) {
|
||||
const auto& Kg = ffGains.Kg.gain;
|
||||
const auto& offset = ffGains.offset.gain;
|
||||
m_quasistaticData.simData = PopulateTimeDomainSim(
|
||||
rawSlow, startTimes, fastStep, sysid::ArmSim{Ks, Kv, Ka, Kg, offset},
|
||||
&simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
|
||||
m_dynamicData.simData = PopulateTimeDomainSim(
|
||||
rawFast, startTimes, fastStep, sysid::ArmSim{Ks, Kv, Ka, Kg, offset},
|
||||
&simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
|
||||
} else {
|
||||
m_quasistaticData.simData = PopulateTimeDomainSim(
|
||||
rawSlow, startTimes, fastStep, sysid::SimpleMotorSim{Ks, Kv, Ka},
|
||||
&simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
|
||||
m_dynamicData.simData = PopulateTimeDomainSim(
|
||||
rawFast, startTimes, fastStep, sysid::SimpleMotorSim{Ks, Kv, Ka},
|
||||
&simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints);
|
||||
}
|
||||
}
|
||||
|
||||
// Acceleration-vs-velocity plot
|
||||
|
||||
// Find minimum velocity of slow and fast datasets, then find point for line
|
||||
// of best fit
|
||||
auto slowMinVel =
|
||||
std::min_element(slow.cbegin(), slow.cend(), [](auto& a, auto& b) {
|
||||
return a.velocity < b.velocity;
|
||||
})->velocity;
|
||||
auto fastMinVel =
|
||||
std::min_element(fast.cbegin(), fast.cend(), [](auto& a, auto& b) {
|
||||
return a.velocity < b.velocity;
|
||||
})->velocity;
|
||||
auto minVel = std::min(slowMinVel, fastMinVel);
|
||||
m_regressionData.fitLine[0] = ImPlotPoint{minVel, -Kv / Ka * minVel};
|
||||
|
||||
// Find maximum velocity of slow and fast datasets, then find point for line
|
||||
// of best fit
|
||||
auto slowMaxVel =
|
||||
std::max_element(slow.cbegin(), slow.cend(), [](auto& a, auto& b) {
|
||||
return a.velocity < b.velocity;
|
||||
})->velocity;
|
||||
auto fastMaxVel =
|
||||
std::max_element(fast.cbegin(), fast.cend(), [](auto& a, auto& b) {
|
||||
return a.velocity < b.velocity;
|
||||
})->velocity;
|
||||
auto maxVel = std::max(slowMaxVel, fastMaxVel);
|
||||
m_regressionData.fitLine[1] = ImPlotPoint{maxVel, -Kv / Ka * maxVel};
|
||||
|
||||
// Populate acceleration vs velocity graph
|
||||
for (size_t i = 0; i < slow.size(); i += slowStep) {
|
||||
if (abort) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Calculate portion of acceleration caused by back-EMF
|
||||
double accelPortion = slow[i].acceleration - 1.0 / Ka * slow[i].voltage +
|
||||
std::copysign(Ks / Ka, slow[i].velocity);
|
||||
|
||||
if (type == analysis::kElevator) {
|
||||
const auto& Kg = ffGains.Kg.gain;
|
||||
accelPortion -= Kg / Ka;
|
||||
} else if (type == analysis::kArm) {
|
||||
const auto& Kg = ffGains.Kg.gain;
|
||||
accelPortion -= Kg / Ka * slow[i].cos;
|
||||
}
|
||||
|
||||
m_regressionData.data.emplace_back(slow[i].velocity, accelPortion);
|
||||
}
|
||||
for (size_t i = 0; i < fast.size(); i += fastStep) {
|
||||
if (abort) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Calculate portion of voltage that corresponds to change in acceleration.
|
||||
double accelPortion = fast[i].acceleration - 1.0 / Ka * fast[i].voltage +
|
||||
std::copysign(Ks / Ka, fast[i].velocity);
|
||||
|
||||
if (type == analysis::kElevator) {
|
||||
const auto& Kg = ffGains.Kg.gain;
|
||||
accelPortion -= Kg / Ka;
|
||||
} else if (type == analysis::kArm) {
|
||||
const auto& Kg = ffGains.Kg.gain;
|
||||
accelPortion -= Kg / Ka * fast[i].cos;
|
||||
}
|
||||
|
||||
m_regressionData.data.emplace_back(fast[i].velocity, accelPortion);
|
||||
}
|
||||
|
||||
// Timestep-vs-time plot
|
||||
|
||||
for (size_t i = 0; i < slow.size(); i += slowStep) {
|
||||
if (i > 0) {
|
||||
// If the current timestamp is not in the startTimes array, it is the
|
||||
// during a test and should be included. If it is in the startTimes
|
||||
// array, it is the beginning of a new test and the dt will be inflated.
|
||||
// Therefore we skip those to exclude that dt and effectively reset dt
|
||||
// calculations.
|
||||
if (slow[i].dt > 0_s &&
|
||||
std::find(startTimes.begin(), startTimes.end(), slow[i].timestamp) ==
|
||||
startTimes.end()) {
|
||||
m_timestepData.data.emplace_back(
|
||||
(slow[i].timestamp).value(),
|
||||
units::millisecond_t{slow[i].dt}.value());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < fast.size(); i += fastStep) {
|
||||
if (i > 0) {
|
||||
// If the current timestamp is not in the startTimes array, it is the
|
||||
// during a test and should be included. If it is in the startTimes
|
||||
// array, it is the beginning of a new test and the dt will be inflated.
|
||||
// Therefore we skip those to exclude that dt and effectively reset dt
|
||||
// calculations.
|
||||
if (fast[i].dt > 0_s &&
|
||||
std::find(startTimes.begin(), startTimes.end(), fast[i].timestamp) ==
|
||||
startTimes.end()) {
|
||||
m_timestepData.data.emplace_back(
|
||||
(fast[i].timestamp).value(),
|
||||
units::millisecond_t{fast[i].dt}.value());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
auto minTime =
|
||||
units::math::min(slow.front().timestamp, fast.front().timestamp);
|
||||
m_timestepData.fitLine[0] =
|
||||
ImPlotPoint{minTime.value(), units::millisecond_t{dtMean}.value()};
|
||||
|
||||
auto maxTime = units::math::max(slow.back().timestamp, fast.back().timestamp);
|
||||
m_timestepData.fitLine[1] =
|
||||
ImPlotPoint{maxTime.value(), units::millisecond_t{dtMean}.value()};
|
||||
|
||||
// RMSE = std::sqrt(sum((x_i - x^_i)^2) / N) where sum represents the sum of
|
||||
// all time series points, x_i represents the velocity at a timestep, x^_i
|
||||
// represents the prediction at the timestep, and N represents the number of
|
||||
// points
|
||||
m_RMSE = std::sqrt(simSquaredErrorSum / timeSeriesPoints);
|
||||
m_accelRSquared =
|
||||
1 - m_RMSE / std::sqrt(squaredVariationSum / timeSeriesPoints);
|
||||
FitPlots();
|
||||
}
|
||||
|
||||
void AnalyzerPlot::FitPlots() {
|
||||
// Set the "fit" flag to true.
|
||||
m_quasistaticData.fitNextPlot = true;
|
||||
m_dynamicData.fitNextPlot = true;
|
||||
m_regressionData.fitNextPlot = true;
|
||||
m_timestepData.fitNextPlot = true;
|
||||
}
|
||||
|
||||
double* AnalyzerPlot::GetSimRMSE() {
|
||||
return &m_RMSE;
|
||||
}
|
||||
|
||||
double* AnalyzerPlot::GetSimRSquared() {
|
||||
return &m_accelRSquared;
|
||||
}
|
||||
|
||||
static void PlotSimData(std::vector<std::vector<ImPlotPoint>>& data) {
|
||||
for (auto&& pts : data) {
|
||||
ImPlot::SetNextLineStyle(IMPLOT_AUTO_COL, 1.5);
|
||||
ImPlot::PlotLineG("Simulation", Getter, pts.data(), pts.size());
|
||||
}
|
||||
}
|
||||
|
||||
bool AnalyzerPlot::DisplayPlots() {
|
||||
std::unique_lock lock(m_mutex, std::defer_lock);
|
||||
|
||||
if (!lock.try_lock()) {
|
||||
ImGui::Text("Loading %c",
|
||||
"|/-\\"[static_cast<int>(ImGui::GetTime() / 0.05f) & 3]);
|
||||
return false;
|
||||
}
|
||||
|
||||
ImVec2 plotSize = ImGui::GetContentRegionAvail();
|
||||
|
||||
// Fit two plots horizontally
|
||||
plotSize.x = (plotSize.x - ImGui::GetStyle().ItemSpacing.x) / 2.f;
|
||||
|
||||
// Fit two plots vertically while leaving room for three text boxes
|
||||
const float textBoxHeight = ImGui::GetFontSize() * 1.75;
|
||||
plotSize.y =
|
||||
(plotSize.y - textBoxHeight * 3 - ImGui::GetStyle().ItemSpacing.y) / 2.f;
|
||||
|
||||
m_quasistaticData.Plot("Quasistatic Velocity vs. Time", plotSize,
|
||||
m_velocityLabel.c_str(), m_pointSize);
|
||||
ImGui::SameLine();
|
||||
m_dynamicData.Plot("Dynamic Velocity vs. Time", plotSize,
|
||||
m_velocityLabel.c_str(), m_pointSize);
|
||||
|
||||
m_regressionData.Plot("Acceleration vs. Velocity", plotSize,
|
||||
m_velocityLabel.c_str(), m_velPortionAccelLabel.c_str(),
|
||||
true, true, m_pointSize);
|
||||
ImGui::SameLine();
|
||||
m_timestepData.Plot("Timesteps vs. Time", plotSize, "Time (s)",
|
||||
"Timestep duration (ms)", true, false, m_pointSize,
|
||||
[] { ImPlot::SetupAxisLimits(ImAxis_Y1, 0, 50); });
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
AnalyzerPlot::FilteredDataVsTimePlot::FilteredDataVsTimePlot() {
|
||||
rawData.reserve(kMaxSize);
|
||||
filteredData.reserve(kMaxSize);
|
||||
simData.reserve(kMaxSize);
|
||||
}
|
||||
|
||||
void AnalyzerPlot::FilteredDataVsTimePlot::Plot(const char* title,
|
||||
const ImVec2& size,
|
||||
const char* yLabel,
|
||||
float pointSize) {
|
||||
// Generate Sim vs Filtered Plot
|
||||
if (fitNextPlot) {
|
||||
ImPlot::SetNextAxesToFit();
|
||||
}
|
||||
|
||||
if (ImPlot::BeginPlot(title, size)) {
|
||||
ImPlot::SetupAxis(ImAxis_X1, "Time (s)", ImPlotAxisFlags_NoGridLines);
|
||||
ImPlot::SetupAxis(ImAxis_Y1, yLabel, ImPlotAxisFlags_NoGridLines);
|
||||
ImPlot::SetupLegend(ImPlotLocation_NorthEast);
|
||||
|
||||
// Plot Raw Data
|
||||
ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0);
|
||||
ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize);
|
||||
ImPlot::PlotScatterG("Raw Data", Getter, rawData.data(), rawData.size());
|
||||
|
||||
// Plot Filtered Data after Raw data
|
||||
ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0);
|
||||
ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize);
|
||||
ImPlot::PlotScatterG("Filtered Data", Getter, filteredData.data(),
|
||||
filteredData.size());
|
||||
|
||||
// Plot Simulation Data for Velocity Data
|
||||
PlotSimData(simData);
|
||||
|
||||
// Disable constant resizing
|
||||
if (fitNextPlot) {
|
||||
fitNextPlot = false;
|
||||
}
|
||||
|
||||
ImPlot::EndPlot();
|
||||
}
|
||||
}
|
||||
|
||||
void AnalyzerPlot::FilteredDataVsTimePlot::Clear() {
|
||||
rawData.clear();
|
||||
filteredData.clear();
|
||||
simData.clear();
|
||||
}
|
||||
|
||||
AnalyzerPlot::DataWithFitLinePlot::DataWithFitLinePlot() {
|
||||
data.reserve(kMaxSize);
|
||||
}
|
||||
|
||||
void AnalyzerPlot::DataWithFitLinePlot::Plot(const char* title,
|
||||
const ImVec2& size,
|
||||
const char* xLabel,
|
||||
const char* yLabel, bool fitX,
|
||||
bool fitY, float pointSize,
|
||||
std::function<void()> setup) {
|
||||
if (fitNextPlot) {
|
||||
if (fitX && fitY) {
|
||||
ImPlot::SetNextAxesToFit();
|
||||
} else if (fitX && !fitY) {
|
||||
ImPlot::SetNextAxisToFit(ImAxis_X1);
|
||||
} else if (!fitX && fitY) {
|
||||
ImPlot::SetNextAxisToFit(ImAxis_Y1);
|
||||
}
|
||||
}
|
||||
|
||||
if (ImPlot::BeginPlot(title, size)) {
|
||||
setup();
|
||||
ImPlot::SetupAxis(ImAxis_X1, xLabel, ImPlotAxisFlags_NoGridLines);
|
||||
ImPlot::SetupAxis(ImAxis_Y1, yLabel, ImPlotAxisFlags_NoGridLines);
|
||||
ImPlot::SetupLegend(ImPlotLocation_NorthEast);
|
||||
|
||||
// Get a reference to the data that we are plotting.
|
||||
ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0);
|
||||
ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize);
|
||||
ImPlot::PlotScatterG("Filtered Data", Getter, data.data(), data.size());
|
||||
|
||||
ImPlot::SetNextLineStyle(IMPLOT_AUTO_COL, 1.5);
|
||||
ImPlot::PlotLineG("Fit", Getter, fitLine.data(), fitLine.size());
|
||||
|
||||
ImPlot::EndPlot();
|
||||
|
||||
if (fitNextPlot) {
|
||||
fitNextPlot = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AnalyzerPlot::DataWithFitLinePlot::Clear() {
|
||||
data.clear();
|
||||
|
||||
// Reset line of best fit
|
||||
fitLine[0] = ImPlotPoint{0, 0};
|
||||
fitLine[1] = ImPlotPoint{0, 0};
|
||||
}
|
||||
312
tools/sysid/src/main/native/cpp/view/DataSelector.cpp
Normal file
312
tools/sysid/src/main/native/cpp/view/DataSelector.cpp
Normal file
@@ -0,0 +1,312 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/view/DataSelector.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <fmt/format.h>
|
||||
#include <imgui.h>
|
||||
#include <wpi/Logger.h>
|
||||
#include <wpi/StringExtras.h>
|
||||
#include <wpi/datalog/DataLogReader.h>
|
||||
#include <wpi/datalog/DataLogReaderThread.h>
|
||||
|
||||
#include "sysid/Util.h"
|
||||
#include "sysid/analysis/AnalysisType.h"
|
||||
#include "sysid/analysis/Storage.h"
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
static constexpr const char* kAnalysisTypes[] = {"Elevator", "Arm", "Simple"};
|
||||
|
||||
static bool EmitEntryTarget(const char* name, bool isString,
|
||||
const wpi::log::DataLogReaderEntry** entry) {
|
||||
if (*entry) {
|
||||
auto text =
|
||||
fmt::format("{}: {} ({})", name, (*entry)->name, (*entry)->type);
|
||||
ImGui::TextUnformatted(text.c_str());
|
||||
} else {
|
||||
ImGui::Text("%s: <none (DROP HERE)> (%s)", name,
|
||||
isString ? "string" : "number");
|
||||
}
|
||||
bool rv = false;
|
||||
if (ImGui::BeginDragDropTarget()) {
|
||||
if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload(
|
||||
isString ? "DataLogEntryString" : "DataLogEntry")) {
|
||||
assert(payload->DataSize == sizeof(const wpi::log::DataLogReaderEntry*));
|
||||
*entry =
|
||||
*static_cast<const wpi::log::DataLogReaderEntry**>(payload->Data);
|
||||
rv = true;
|
||||
}
|
||||
ImGui::EndDragDropTarget();
|
||||
}
|
||||
return rv;
|
||||
}
|
||||
|
||||
void DataSelector::Display() {
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// building test data is modal (due to async access)
|
||||
if (m_testdataFuture.valid()) {
|
||||
if (m_testdataFuture.wait_for(0s) == std::future_status::ready) {
|
||||
TestData data = m_testdataFuture.get();
|
||||
for (auto&& motordata : data.motorData) {
|
||||
m_testdataStats.emplace_back(
|
||||
fmt::format("Test State: {}", motordata.first));
|
||||
int i = 0;
|
||||
for (auto&& run : motordata.second.runs) {
|
||||
m_testdataStats.emplace_back(fmt::format(
|
||||
" Run {} samples: {} Volt {} Pos {} Vel", ++i,
|
||||
run.voltage.size(), run.position.size(), run.velocity.size()));
|
||||
}
|
||||
}
|
||||
if (testdata) {
|
||||
testdata(std::move(data));
|
||||
}
|
||||
}
|
||||
ImGui::Text("Loading data...");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!m_testdataStats.empty()) {
|
||||
for (auto&& line : m_testdataStats) {
|
||||
ImGui::TextUnformatted(line.c_str());
|
||||
}
|
||||
if (ImGui::Button("Ok")) {
|
||||
m_testdataStats.clear();
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (EmitEntryTarget("Test State", true, &m_testStateEntry)) {
|
||||
m_testsFuture =
|
||||
std::async(std::launch::async, [testStateEntry = m_testStateEntry] {
|
||||
return LoadTests(*testStateEntry);
|
||||
});
|
||||
}
|
||||
|
||||
if (!m_testStateEntry) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (m_testsFuture.valid() &&
|
||||
m_testsFuture.wait_for(0s) == std::future_status::ready) {
|
||||
m_tests = m_testsFuture.get();
|
||||
for (auto it = m_tests.begin(); it != m_tests.end();) {
|
||||
if (it->first != "quasistatic" && it->first != "dynamic") {
|
||||
WPI_WARNING(m_logger, "Unrecognized test {}, removing", it->first);
|
||||
it = m_tests.erase(it);
|
||||
continue;
|
||||
}
|
||||
for (auto it2 = it->second.begin(); it2 != it->second.end();) {
|
||||
auto direction = wpi::rsplit(it2->first, '-').second;
|
||||
if (direction != "forward" && direction != "reverse") {
|
||||
WPI_WARNING(m_logger, "Unrecognized direction {}, removing",
|
||||
direction);
|
||||
it2 = it->second.erase(it2);
|
||||
continue;
|
||||
}
|
||||
WPI_INFO(m_logger, "Loaded test state {}", it2->first);
|
||||
m_executedTests.insert(it2->first);
|
||||
++it2;
|
||||
}
|
||||
if (it->second.empty()) {
|
||||
WPI_WARNING(m_logger, "No data for test {}, removing", it->first);
|
||||
it = m_tests.erase(it);
|
||||
continue;
|
||||
}
|
||||
++it;
|
||||
}
|
||||
WPI_INFO(m_logger, "Loaded {} tests", m_tests.size());
|
||||
}
|
||||
|
||||
if (m_tests.empty()) {
|
||||
if (m_testsFuture.valid()) {
|
||||
ImGui::TextUnformatted("Reading tests...");
|
||||
} else {
|
||||
ImGui::TextUnformatted("No tests found");
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (m_executedTests.size() < 4 && !m_testCountValidated) {
|
||||
for (auto test : kValidTests) {
|
||||
if (!m_executedTests.contains(test)) {
|
||||
m_missingTests.push_back(test);
|
||||
m_testCountValidated = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if 0
|
||||
// Test filtering
|
||||
if (ImGui::BeginCombo("Test", m_selectedTest.c_str())) {
|
||||
for (auto&& test : m_tests) {
|
||||
if (ImGui::Selectable(test.first.c_str(), test.first == m_selectedTest)) {
|
||||
m_selectedTest = test.first;
|
||||
}
|
||||
}
|
||||
ImGui::EndCombo();
|
||||
}
|
||||
#endif
|
||||
|
||||
ImGui::Combo("Analysis Type", &m_selectedAnalysis, kAnalysisTypes,
|
||||
IM_ARRAYSIZE(kAnalysisTypes));
|
||||
|
||||
// DND targets
|
||||
EmitEntryTarget("Velocity", false, &m_velocityEntry);
|
||||
EmitEntryTarget("Position", false, &m_positionEntry);
|
||||
EmitEntryTarget("Voltage", false, &m_voltageEntry);
|
||||
|
||||
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7);
|
||||
ImGui::Combo("Units", &m_selectedUnit, kUnits, IM_ARRAYSIZE(kUnits));
|
||||
|
||||
ImGui::InputDouble("Velocity scaling", &m_velocityScale);
|
||||
ImGui::InputDouble("Position scaling", &m_positionScale);
|
||||
|
||||
if (/*!m_selectedTest.empty() &&*/ m_velocityEntry && m_positionEntry &&
|
||||
m_voltageEntry) {
|
||||
if (ImGui::Button("Load")) {
|
||||
m_testdataFuture =
|
||||
std::async(std::launch::async, [this] { return BuildTestData(); });
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void DataSelector::Reset() {
|
||||
m_testsFuture = {};
|
||||
m_tests.clear();
|
||||
m_selectedTest.clear();
|
||||
m_testStateEntry = nullptr;
|
||||
m_velocityEntry = nullptr;
|
||||
m_positionEntry = nullptr;
|
||||
m_voltageEntry = nullptr;
|
||||
m_testdataFuture = {};
|
||||
}
|
||||
|
||||
DataSelector::Tests DataSelector::LoadTests(
|
||||
const wpi::log::DataLogReaderEntry& testStateEntry) {
|
||||
Tests tests;
|
||||
for (auto&& range : testStateEntry.ranges) {
|
||||
std::string_view prevState;
|
||||
Runs* curRuns = nullptr;
|
||||
wpi::log::DataLogReader::iterator lastStart = range.begin();
|
||||
int64_t ts = lastStart->GetTimestamp();
|
||||
for (auto it = range.begin(), end = range.end(); it != end; ++it) {
|
||||
ts = it->GetTimestamp();
|
||||
std::string_view testState;
|
||||
if (it->GetEntry() != testStateEntry.entry ||
|
||||
!it->GetString(&testState)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// track runs as iterator ranges of the same test
|
||||
if (testState != prevState) {
|
||||
if (curRuns) {
|
||||
curRuns->emplace_back(lastStart->GetTimestamp(), ts);
|
||||
}
|
||||
lastStart = it;
|
||||
}
|
||||
prevState = testState;
|
||||
|
||||
if (testState == "none") {
|
||||
curRuns = nullptr;
|
||||
continue;
|
||||
}
|
||||
|
||||
auto [testName, direction] = wpi::rsplit(testState, '-');
|
||||
auto testIt = tests.find(testName);
|
||||
if (testIt == tests.end()) {
|
||||
testIt = tests.emplace(std::string{testName}, State{}).first;
|
||||
}
|
||||
auto stateIt = testIt->second.find(testState);
|
||||
if (stateIt == testIt->second.end()) {
|
||||
stateIt = testIt->second.emplace(std::string{testState}, Runs{}).first;
|
||||
}
|
||||
curRuns = &stateIt->second;
|
||||
}
|
||||
|
||||
if (curRuns) {
|
||||
curRuns->emplace_back(lastStart->GetTimestamp(), ts);
|
||||
}
|
||||
}
|
||||
return tests;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
static void AddSamples(std::vector<MotorData::Run::Sample<T>>& samples,
|
||||
const std::vector<std::pair<int64_t, double>>& data,
|
||||
int64_t tsbegin, int64_t tsend) {
|
||||
// data is sorted, so do a binary search for tsbegin and tsend
|
||||
auto begin = std::lower_bound(
|
||||
data.begin(), data.end(), tsbegin,
|
||||
[](const auto& datapoint, double val) { return datapoint.first < val; });
|
||||
auto end = std::lower_bound(
|
||||
begin, data.end(), tsend,
|
||||
[](const auto& datapoint, double val) { return datapoint.first < val; });
|
||||
|
||||
for (auto it = begin; it != end; ++it) {
|
||||
samples.emplace_back(units::second_t{it->first * 1.0e-6}, T{it->second});
|
||||
}
|
||||
}
|
||||
|
||||
static std::vector<std::pair<int64_t, double>> GetData(
|
||||
const wpi::log::DataLogReaderEntry& entry, double scale) {
|
||||
std::vector<std::pair<int64_t, double>> rv;
|
||||
bool isDouble = entry.type == "double";
|
||||
for (auto&& range : entry.ranges) {
|
||||
for (auto&& record : range) {
|
||||
if (record.GetEntry() != entry.entry) {
|
||||
continue;
|
||||
}
|
||||
if (isDouble) {
|
||||
double val;
|
||||
if (record.GetDouble(&val)) {
|
||||
rv.emplace_back(record.GetTimestamp(), val * scale);
|
||||
}
|
||||
} else {
|
||||
float val;
|
||||
if (record.GetFloat(&val)) {
|
||||
rv.emplace_back(record.GetTimestamp(),
|
||||
static_cast<double>(val * scale));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::sort(rv.begin(), rv.end(),
|
||||
[](const auto& a, const auto& b) { return a.first < b.first; });
|
||||
return rv;
|
||||
}
|
||||
|
||||
TestData DataSelector::BuildTestData() {
|
||||
TestData data;
|
||||
data.distanceUnit = kUnits[m_selectedUnit];
|
||||
data.mechanismType = analysis::FromName(kAnalysisTypes[m_selectedAnalysis]);
|
||||
|
||||
// read and sort the entire dataset first; this is memory hungry but
|
||||
// dramatically speeds up splitting it into runs.
|
||||
auto voltageData = GetData(*m_voltageEntry, 1.0);
|
||||
auto positionData = GetData(*m_positionEntry, m_positionScale);
|
||||
auto velocityData = GetData(*m_velocityEntry, m_velocityScale);
|
||||
|
||||
for (auto&& test : m_tests) {
|
||||
for (auto&& state : test.second) {
|
||||
auto& motorData = data.motorData[state.first];
|
||||
for (auto [tsbegin, tsend] : state.second) {
|
||||
auto& run = motorData.runs.emplace_back();
|
||||
AddSamples(run.voltage, voltageData, tsbegin, tsend);
|
||||
AddSamples(run.position, positionData, tsbegin, tsend);
|
||||
AddSamples(run.velocity, velocityData, tsbegin, tsend);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return data;
|
||||
}
|
||||
213
tools/sysid/src/main/native/cpp/view/LogLoader.cpp
Normal file
213
tools/sysid/src/main/native/cpp/view/LogLoader.cpp
Normal file
@@ -0,0 +1,213 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// 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 "sysid/view/LogLoader.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <imgui.h>
|
||||
#include <imgui_stdlib.h>
|
||||
#include <portable-file-dialogs.h>
|
||||
#include <wpi/SmallVector.h>
|
||||
#include <wpi/SpanExtras.h>
|
||||
#include <wpi/StringExtras.h>
|
||||
#include <wpi/datalog/DataLogReaderThread.h>
|
||||
#include <wpi/fs.h>
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
LogLoader::LogLoader(glass::Storage& storage, wpi::Logger& logger) {}
|
||||
|
||||
LogLoader::~LogLoader() = default;
|
||||
|
||||
void LogLoader::Display() {
|
||||
if (ImGui::Button("Open data log file...")) {
|
||||
m_opener = std::make_unique<pfd::open_file>(
|
||||
"Select Data Log", "",
|
||||
std::vector<std::string>{"DataLog Files", "*.wpilog"});
|
||||
}
|
||||
|
||||
// Handle opening the file
|
||||
if (m_opener && m_opener->ready(0)) {
|
||||
if (!m_opener->result().empty()) {
|
||||
m_filename = m_opener->result()[0];
|
||||
|
||||
auto fileBuffer = wpi::MemoryBuffer::GetFile(m_filename);
|
||||
if (!fileBuffer) {
|
||||
ImGui::OpenPopup("Error");
|
||||
m_error = fmt::format("Could not open file: {}",
|
||||
fileBuffer.error().message());
|
||||
return;
|
||||
}
|
||||
|
||||
wpi::log::DataLogReader reader{std::move(*fileBuffer)};
|
||||
if (!reader.IsValid()) {
|
||||
ImGui::OpenPopup("Error");
|
||||
m_error = "Not a valid datalog file";
|
||||
return;
|
||||
}
|
||||
unload();
|
||||
m_reader =
|
||||
std::make_unique<wpi::log::DataLogReaderThread>(std::move(reader));
|
||||
m_entryTree.clear();
|
||||
}
|
||||
m_opener.reset();
|
||||
}
|
||||
|
||||
// Handle errors
|
||||
ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f));
|
||||
if (ImGui::BeginPopupModal("Error")) {
|
||||
ImGui::PushTextWrapPos(0.0f);
|
||||
ImGui::TextUnformatted(m_error.c_str());
|
||||
ImGui::PopTextWrapPos();
|
||||
if (ImGui::Button("Close")) {
|
||||
ImGui::CloseCurrentPopup();
|
||||
}
|
||||
ImGui::EndPopup();
|
||||
}
|
||||
|
||||
if (!m_reader) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Summary info
|
||||
ImGui::TextUnformatted(fs::path{m_filename}.stem().string().c_str());
|
||||
ImGui::Text("%u records, %u entries%s", m_reader->GetNumRecords(),
|
||||
m_reader->GetNumEntries(),
|
||||
m_reader->IsDone() ? "" : " (working)");
|
||||
|
||||
if (!m_reader->IsDone()) {
|
||||
return;
|
||||
}
|
||||
|
||||
bool refilter = ImGui::InputText("Filter", &m_filter);
|
||||
|
||||
// Display tree of entries
|
||||
if (m_entryTree.empty() || refilter) {
|
||||
RebuildEntryTree();
|
||||
}
|
||||
|
||||
ImGui::BeginTable(
|
||||
"Entries", 2,
|
||||
ImGuiTableFlags_Borders | ImGuiTableFlags_SizingStretchProp);
|
||||
ImGui::TableSetupColumn("Name");
|
||||
ImGui::TableSetupColumn("Type");
|
||||
// ImGui::TableSetupColumn("Metadata");
|
||||
ImGui::TableHeadersRow();
|
||||
DisplayEntryTree(m_entryTree);
|
||||
ImGui::EndTable();
|
||||
}
|
||||
|
||||
void LogLoader::RebuildEntryTree() {
|
||||
m_entryTree.clear();
|
||||
wpi::SmallVector<std::string_view, 16> parts;
|
||||
m_reader->ForEachEntryName([&](const wpi::log::DataLogReaderEntry& entry) {
|
||||
// only show double/float/string entries (TODO: support struct/protobuf)
|
||||
if (entry.type != "double" && entry.type != "float" &&
|
||||
entry.type != "string") {
|
||||
return;
|
||||
}
|
||||
|
||||
// filter on name
|
||||
if (!m_filter.empty() && !wpi::contains_lower(entry.name, m_filter)) {
|
||||
return;
|
||||
}
|
||||
|
||||
parts.clear();
|
||||
// split on first : if one is present
|
||||
auto [prefix, mainpart] = wpi::split(entry.name, ':');
|
||||
if (mainpart.empty() || wpi::contains(prefix, '/')) {
|
||||
mainpart = entry.name;
|
||||
} else {
|
||||
parts.emplace_back(prefix);
|
||||
}
|
||||
wpi::split(mainpart, '/', -1, false,
|
||||
[&](auto part) { parts.emplace_back(part); });
|
||||
|
||||
// ignore a raw "/" key
|
||||
if (parts.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// get to leaf
|
||||
auto nodes = &m_entryTree;
|
||||
for (auto part : wpi::drop_back(std::span{parts.begin(), parts.end()})) {
|
||||
auto it =
|
||||
std::find_if(nodes->begin(), nodes->end(),
|
||||
[&](const auto& node) { return node.name == part; });
|
||||
if (it == nodes->end()) {
|
||||
nodes->emplace_back(part);
|
||||
// path is from the beginning of the string to the end of the current
|
||||
// part; this works because part is a reference to the internals of
|
||||
// entry.name
|
||||
nodes->back().path.assign(
|
||||
entry.name.data(), part.data() + part.size() - entry.name.data());
|
||||
it = nodes->end() - 1;
|
||||
}
|
||||
nodes = &it->children;
|
||||
}
|
||||
|
||||
auto it = std::find_if(nodes->begin(), nodes->end(), [&](const auto& node) {
|
||||
return node.name == parts.back();
|
||||
});
|
||||
if (it == nodes->end()) {
|
||||
nodes->emplace_back(parts.back());
|
||||
// no need to set path, as it's identical to entry.name
|
||||
it = nodes->end() - 1;
|
||||
}
|
||||
it->entry = &entry;
|
||||
});
|
||||
}
|
||||
|
||||
static void EmitEntry(const std::string& name,
|
||||
const wpi::log::DataLogReaderEntry& entry) {
|
||||
ImGui::TableNextColumn();
|
||||
ImGui::Selectable(name.c_str());
|
||||
if (ImGui::BeginDragDropSource()) {
|
||||
auto entryPtr = &entry;
|
||||
ImGui::SetDragDropPayload(
|
||||
entry.type == "string" ? "DataLogEntryString" : "DataLogEntry",
|
||||
&entryPtr,
|
||||
sizeof(entryPtr)); // NOLINT
|
||||
ImGui::TextUnformatted(entry.name.data(),
|
||||
entry.name.data() + entry.name.size());
|
||||
ImGui::EndDragDropSource();
|
||||
}
|
||||
ImGui::TableNextColumn();
|
||||
ImGui::TextUnformatted(entry.type.data(),
|
||||
entry.type.data() + entry.type.size());
|
||||
#if 0
|
||||
ImGui::TableNextColumn();
|
||||
ImGui::TextUnformatted(entry.metadata.data(),
|
||||
entry.metadata.data() + entry.metadata.size());
|
||||
#endif
|
||||
}
|
||||
|
||||
void LogLoader::DisplayEntryTree(const std::vector<EntryTreeNode>& tree) {
|
||||
for (auto&& node : tree) {
|
||||
if (node.entry) {
|
||||
EmitEntry(node.name, *node.entry);
|
||||
}
|
||||
|
||||
if (!node.children.empty()) {
|
||||
ImGui::TableNextColumn();
|
||||
bool open = ImGui::TreeNodeEx(node.name.c_str(),
|
||||
ImGuiTreeNodeFlags_SpanFullWidth);
|
||||
ImGui::TableNextColumn();
|
||||
#if 0
|
||||
ImGui::TableNextColumn();
|
||||
#endif
|
||||
if (open) {
|
||||
DisplayEntryTree(node.children);
|
||||
ImGui::TreePop();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user