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

[sysid] Load DataLog files directly for analysis (#6103)

Browse Source 
Co-authored-by: Oblarg <emichaelbrnett@gmail.com>
This commit is contained in:
Peter Johnson
2024-01-05 16:24:31 -08:00
committed by GitHub
parent f94e3d81b9
commit 7c26bc70ab
33 changed files with 1086 additions and 2013 deletions
Download Patch File Download Diff File Expand all files Collapse all files

495
sysid/src/main/native/cpp/analysis/AnalysisManager.cpp
View File

@@ -5,24 +5,39 @@
#include "sysid/analysis/AnalysisManager.h"
#include <cmath>
#include <cstddef>
#include <functional>
#include <stdexcept>
#include <fmt/format.h>
#include <units/angle.h>
#include <units/math.h>
#include <wpi/MathExtras.h>
#include <wpi/MemoryBuffer.h>
#include <wpi/StringExtras.h>
#include <wpi/StringMap.h>
#include "sysid/Util.h"
#include "sysid/analysis/FilteringUtils.h"
#include "sysid/analysis/JSONConverter.h"
#include "sysid/analysis/TrackWidthAnalysis.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.
@@ -38,18 +53,25 @@ using namespace sysid;
*
* @return A PreparedData vector
*/
template <size_t S, size_t Timestamp, size_t Voltage, size_t Position,
size_t Velocity>
static std::vector<PreparedData> ConvertToPrepared(
const std::vector<std::array<double, S>>& data) {
static std::vector<PreparedData> ConvertToPrepared(const MotorData& data) {
std::vector<PreparedData> prepared;
for (int i = 0; i < static_cast<int>(data.size()) - 1; ++i) {
const auto& pt1 = data[i];
const auto& pt2 = data[i + 1];
prepared.emplace_back(PreparedData{
units::second_t{pt1[Timestamp]}, pt1[Voltage], pt1[Position],
pt1[Velocity], units::second_t{pt2[Timestamp] - pt1[Timestamp]}});
// 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;
}
@@ -62,18 +84,16 @@ static std::vector<PreparedData> ConvertToPrepared(
*
* @param dataset A reference to the dataset being used
*/
template <size_t S>
static void CopyRawData(
wpi::StringMap<std::vector<std::array<double, S>>>* dataset) {
static void CopyRawData(wpi::StringMap<MotorData>* dataset) {
auto& data = *dataset;
// Loads the Raw Data
for (auto& it : data) {
auto key = it.first();
auto& dataset = it.getValue();
auto& motorData = it.getValue();
if (!wpi::contains(key, "raw")) {
data[fmt::format("raw-{}", key)] = dataset;
data[fmt::format("original-raw-{}", key)] = dataset;
data[fmt::format("raw-{}", key)] = motorData;
data[fmt::format("original-raw-{}", key)] = motorData;
}
}
}
@@ -94,416 +114,73 @@ static Storage CombineDatasets(const std::vector<PreparedData>& slowForward,
}
void AnalysisManager::PrepareGeneralData() {
using Data = std::array<double, 4>;
wpi::StringMap<std::vector<Data>> data;
wpi::StringMap<std::vector<PreparedData>> preparedData;
// Store the raw data columns.
constexpr size_t kTimeCol = 0;
constexpr size_t kVoltageCol = 1;
constexpr size_t kPosCol = 2;
constexpr size_t kVelCol = 3;
WPI_INFO(m_logger, "{}", "Reading JSON data.");
// Get the major components from the JSON and store them inside a StringMap.
for (auto&& key : AnalysisManager::kJsonDataKeys) {
data[key] = m_json.at(key).get<std::vector<Data>>();
}
WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
// Ensure that voltage and velocity have the same sign. Also multiply
// positions and velocities by the factor.
for (auto it = data.begin(); it != data.end(); ++it) {
for (auto&& pt : it->second) {
pt[kVoltageCol] = std::copysign(pt[kVoltageCol], pt[kVelCol]);
pt[kPosCol] *= m_factor;
pt[kVelCol] *= m_factor;
}
}
WPI_INFO(m_logger, "{}", "Copying raw data.");
CopyRawData(&data);
CopyRawData(&m_data.motorData);
WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct.");
// Convert data to PreparedData structs
for (auto& it : data) {
for (auto& it : m_data.motorData) {
auto key = it.first();
preparedData[key] =
ConvertToPrepared<4, kTimeCol, kVoltageCol, kPosCol, kVelCol>(
data[key]);
preparedData[key] = ConvertToPrepared(m_data.motorData[key]);
WPI_INFO(m_logger, "SAMPLES {}", preparedData[key].size());
}
// Store the original datasets
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(preparedData["original-raw-slow-forward"],
preparedData["original-raw-slow-backward"],
preparedData["original-raw-fast-forward"],
preparedData["original-raw-fast-backward"]);
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_unit);
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[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(
preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
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[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(
preparedData["slow-forward"], preparedData["slow-backward"],
preparedData["fast-forward"], preparedData["fast-backward"]);
m_filteredDataset = CombineDatasets(
preparedData["quasistatic-forward"], preparedData["quasistatic-reverse"],
preparedData["dynamic-forward"], preparedData["dynamic-reverse"]);
m_startTimes = {preparedData["raw-slow-forward"][0].timestamp,
preparedData["raw-slow-backward"][0].timestamp,
preparedData["raw-fast-forward"][0].timestamp,
preparedData["raw-fast-backward"][0].timestamp};
}
void AnalysisManager::PrepareAngularDrivetrainData() {
using Data = std::array<double, 9>;
wpi::StringMap<std::vector<Data>> data;
wpi::StringMap<std::vector<PreparedData>> preparedData;
// Store the relevant raw data columns.
constexpr size_t kTimeCol = 0;
constexpr size_t kLVoltageCol = 1;
constexpr size_t kRVoltageCol = 2;
constexpr size_t kLPosCol = 3;
constexpr size_t kRPosCol = 4;
constexpr size_t kLVelCol = 5;
constexpr size_t kRVelCol = 6;
constexpr size_t kAngleCol = 7;
constexpr size_t kAngularRateCol = 8;
WPI_INFO(m_logger, "{}", "Reading JSON data.");
// Get the major components from the JSON and store them inside a StringMap.
for (auto&& key : AnalysisManager::kJsonDataKeys) {
data[key] = m_json.at(key).get<std::vector<Data>>();
}
WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
// Ensure that voltage and velocity have the same sign. Also multiply
// positions and velocities by the factor.
for (auto it = data.begin(); it != data.end(); ++it) {
for (auto&& pt : it->second) {
pt[kLPosCol] *= m_factor;
pt[kRPosCol] *= m_factor;
pt[kLVelCol] *= m_factor;
pt[kRVelCol] *= m_factor;
// Stores the average voltages in the left voltage column.
// This aggregates the left and right voltages into a single voltage
// column for the ConvertToPrepared() method. std::copysign() ensures the
// polarity of the voltage matches the direction the robot turns.
pt[kLVoltageCol] = std::copysign(
(std::abs(pt[kLVoltageCol]) + std::abs(pt[kRVoltageCol])) / 2,
pt[kAngularRateCol]);
// ω = (v_r - v_l) / trackwidth
// v = ωr => v = ω * trackwidth / 2
// (v_r - v_l) / trackwidth * (trackwidth / 2) = (v_r - v_l) / 2
// However, since we know this is an angular test, the left and right
// wheel velocities will have opposite signs, allowing us to add their
// absolute values and get the same result (in terms of magnitude).
// std::copysign() is used to make sure the direction of the wheel
// velocities matches the direction the robot turns.
pt[kAngularRateCol] =
std::copysign((std::abs(pt[kRVelCol]) + std::abs(pt[kLVelCol])) / 2,
pt[kAngularRateCol]);
}
}
WPI_INFO(m_logger, "{}", "Calculating trackwidth");
// Aggregating all the deltas from all the tests
double leftDelta = 0.0;
double rightDelta = 0.0;
double angleDelta = 0.0;
for (const auto& it : data) {
auto key = it.first();
auto& trackWidthData = data[key];
leftDelta += std::abs(trackWidthData.back()[kLPosCol] -
trackWidthData.front()[kLPosCol]);
rightDelta += std::abs(trackWidthData.back()[kRPosCol] -
trackWidthData.front()[kRPosCol]);
angleDelta += std::abs(trackWidthData.back()[kAngleCol] -
trackWidthData.front()[kAngleCol]);
}
m_trackWidth = sysid::CalculateTrackWidth(leftDelta, rightDelta,
units::radian_t{angleDelta});
WPI_INFO(m_logger, "{}", "Copying raw data.");
CopyRawData(&data);
WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
// Convert raw data to prepared data
for (const auto& it : data) {
auto key = it.first();
preparedData[key] = ConvertToPrepared<9, kTimeCol, kLVoltageCol, kAngleCol,
kAngularRateCol>(data[key]);
}
// Create the distinct datasets and store them
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(preparedData["original-raw-slow-forward"],
preparedData["original-raw-slow-backward"],
preparedData["original-raw-fast-forward"],
preparedData["original-raw-fast-backward"]);
WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
m_velocityDelays, m_minStepTime, m_maxStepTime);
WPI_INFO(m_logger, "{}", "Acceleration filtering.");
sysid::AccelFilter(&preparedData);
WPI_INFO(m_logger, "{}", "Storing datasets.");
// Create the distinct datasets and store them
m_rawDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(
preparedData["raw-slow-forward"], preparedData["raw-slow-backward"],
preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]);
m_filteredDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(
preparedData["slow-forward"], preparedData["slow-backward"],
preparedData["fast-forward"], preparedData["fast-backward"]);
m_startTimes = {preparedData["slow-forward"][0].timestamp,
preparedData["slow-backward"][0].timestamp,
preparedData["fast-forward"][0].timestamp,
preparedData["fast-backward"][0].timestamp};
}
void AnalysisManager::PrepareLinearDrivetrainData() {
using Data = std::array<double, 9>;
wpi::StringMap<std::vector<Data>> data;
wpi::StringMap<std::vector<PreparedData>> preparedData;
// Store the relevant raw data columns.
constexpr size_t kTimeCol = 0;
constexpr size_t kLVoltageCol = 1;
constexpr size_t kRVoltageCol = 2;
constexpr size_t kLPosCol = 3;
constexpr size_t kRPosCol = 4;
constexpr size_t kLVelCol = 5;
constexpr size_t kRVelCol = 6;
// Get the major components from the JSON and store them inside a StringMap.
WPI_INFO(m_logger, "{}", "Reading JSON data.");
for (auto&& key : AnalysisManager::kJsonDataKeys) {
data[key] = m_json.at(key).get<std::vector<Data>>();
}
// Ensure that voltage and velocity have the same sign. Also multiply
// positions and velocities by the factor.
WPI_INFO(m_logger, "{}", "Preprocessing raw data.");
for (auto it = data.begin(); it != data.end(); ++it) {
for (auto&& pt : it->second) {
pt[kLVoltageCol] = std::copysign(pt[kLVoltageCol], pt[kLVelCol]);
pt[kRVoltageCol] = std::copysign(pt[kRVoltageCol], pt[kRVelCol]);
pt[kLPosCol] *= m_factor;
pt[kRPosCol] *= m_factor;
pt[kLVelCol] *= m_factor;
pt[kRVelCol] *= m_factor;
}
}
WPI_INFO(m_logger, "{}", "Copying raw data.");
CopyRawData(&data);
// Convert data to PreparedData
WPI_INFO(m_logger, "{}", "Converting to PreparedData struct.");
for (auto& it : data) {
auto key = it.first();
preparedData[fmt::format("left-{}", key)] =
ConvertToPrepared<9, kTimeCol, kLVoltageCol, kLPosCol, kLVelCol>(
data[key]);
preparedData[fmt::format("right-{}", key)] =
ConvertToPrepared<9, kTimeCol, kRVoltageCol, kRPosCol, kRVelCol>(
data[key]);
}
// Create the distinct raw datasets and store them
auto originalSlowForward = AnalysisManager::DataConcat(
preparedData["left-original-raw-slow-forward"],
preparedData["right-original-raw-slow-forward"]);
auto originalSlowBackward = AnalysisManager::DataConcat(
preparedData["left-original-raw-slow-backward"],
preparedData["right-original-raw-slow-backward"]);
auto originalFastForward = AnalysisManager::DataConcat(
preparedData["left-original-raw-fast-forward"],
preparedData["right-original-raw-fast-forward"]);
auto originalFastBackward = AnalysisManager::DataConcat(
preparedData["left-original-raw-fast-backward"],
preparedData["right-original-raw-fast-backward"]);
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(originalSlowForward, originalSlowBackward,
originalFastForward, originalFastBackward);
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
CombineDatasets(preparedData["left-original-raw-slow-forward"],
preparedData["left-original-raw-slow-backward"],
preparedData["left-original-raw-fast-forward"],
preparedData["left-original-raw-fast-backward"]);
m_originalDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kRight)] =
CombineDatasets(preparedData["right-original-raw-slow-forward"],
preparedData["right-original-raw-slow-backward"],
preparedData["right-original-raw-fast-forward"],
preparedData["right-original-raw-fast-backward"]);
WPI_INFO(m_logger, "{}", "Applying trimming and filtering.");
sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays,
m_velocityDelays, m_minStepTime, m_maxStepTime);
auto slowForward = AnalysisManager::DataConcat(
preparedData["left-slow-forward"], preparedData["right-slow-forward"]);
auto slowBackward = AnalysisManager::DataConcat(
preparedData["left-slow-backward"], preparedData["right-slow-backward"]);
auto fastForward = AnalysisManager::DataConcat(
preparedData["left-fast-forward"], preparedData["right-fast-forward"]);
auto fastBackward = AnalysisManager::DataConcat(
preparedData["left-fast-backward"], preparedData["right-fast-backward"]);
WPI_INFO(m_logger, "{}", "Acceleration filtering.");
sysid::AccelFilter(&preparedData);
WPI_INFO(m_logger, "{}", "Storing datasets.");
// Create the distinct raw datasets and store them
auto rawSlowForward =
AnalysisManager::DataConcat(preparedData["left-raw-slow-forward"],
preparedData["right-raw-slow-forward"]);
auto rawSlowBackward =
AnalysisManager::DataConcat(preparedData["left-raw-slow-backward"],
preparedData["right-raw-slow-backward"]);
auto rawFastForward =
AnalysisManager::DataConcat(preparedData["left-raw-fast-forward"],
preparedData["right-raw-fast-forward"]);
auto rawFastBackward =
AnalysisManager::DataConcat(preparedData["left-raw-fast-backward"],
preparedData["right-raw-fast-backward"]);
m_rawDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(rawSlowForward, rawSlowBackward, rawFastForward,
rawFastBackward);
m_rawDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
CombineDatasets(preparedData["left-raw-slow-forward"],
preparedData["left-raw-slow-backward"],
preparedData["left-raw-fast-forward"],
preparedData["left-raw-fast-backward"]);
m_rawDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kRight)] =
CombineDatasets(preparedData["right-raw-slow-forward"],
preparedData["right-raw-slow-backward"],
preparedData["right-raw-fast-forward"],
preparedData["right-raw-fast-backward"]);
// Create the distinct filtered datasets and store them
m_filteredDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(slowForward, slowBackward, fastForward, fastBackward);
m_filteredDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kLeft)] =
CombineDatasets(preparedData["left-slow-forward"],
preparedData["left-slow-backward"],
preparedData["left-fast-forward"],
preparedData["left-fast-backward"]);
m_filteredDataset[static_cast<int>(
AnalysisManager::Settings::DrivetrainDataset::kRight)] =
CombineDatasets(preparedData["right-slow-forward"],
preparedData["right-slow-backward"],
preparedData["right-fast-forward"],
preparedData["right-fast-backward"]);
m_startTimes = {
rawSlowForward.front().timestamp, rawSlowBackward.front().timestamp,
rawFastForward.front().timestamp, rawFastBackward.front().timestamp};
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},
m_type{analysis::kSimple},
m_unit{"Meters"},
m_factor{1} {}
: m_logger{logger}, m_settings{settings} {}
AnalysisManager::AnalysisManager(std::string_view path, Settings& settings,
AnalysisManager::AnalysisManager(TestData data, Settings& settings,
wpi::Logger& logger)
: m_logger{logger}, m_settings{settings} {
{
// Read JSON from the specified path
std::error_code ec;
std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
wpi::MemoryBuffer::GetFile(path, ec);
if (fileBuffer == nullptr || ec) {
throw FileReadingError(path);
}
m_json = wpi::json::parse(fileBuffer->GetCharBuffer());
WPI_INFO(m_logger, "Read {}", path);
}
// Check that we have a sysid JSON
if (m_json.find("sysid") == m_json.end()) {
// If it's not a sysid JSON, try converting it from frc-char format
std::string newPath = sysid::ConvertJSON(path, logger);
// Read JSON from the specified path
std::error_code ec;
std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
wpi::MemoryBuffer::GetFile(path, ec);
if (fileBuffer == nullptr || ec) {
throw FileReadingError(newPath);
}
m_json = wpi::json::parse(fileBuffer->GetCharBuffer());
WPI_INFO(m_logger, "Read {}", newPath);
}
WPI_INFO(m_logger, "Parsing initial data of {}", path);
// Get the analysis type from the JSON.
m_type = sysid::analysis::FromName(m_json.at("test").get<std::string>());
// Get the rotation -> output units factor from the JSON.
m_unit = m_json.at("units").get<std::string>();
m_factor = m_json.at("unitsPerRotation").get<double>();
WPI_DEBUG(m_logger, "Parsing units per rotation as {} {} per rotation",
m_factor, m_unit);
: m_data{std::move(data)}, m_logger{logger}, m_settings{settings} {
// Reset settings for Dynamic Test Limits
m_settings.stepTestDuration = units::second_t{0.0};
m_settings.motionThreshold = std::numeric_limits<double>::infinity();
}
void AnalysisManager::PrepareData() {
WPI_INFO(m_logger, "Preparing {} data", m_type.name);
if (m_type == analysis::kDrivetrain) {
PrepareLinearDrivetrainData();
} else if (m_type == analysis::kDrivetrainAngular) {
PrepareAngularDrivetrainData();
} else {
PrepareGeneralData();
}
// WPI_INFO(m_logger, "Preparing {} data", m_data.mechanismType.name);
PrepareGeneralData();
WPI_INFO(m_logger, "{}", "Finished Preparing Data");
}
@@ -515,8 +192,9 @@ AnalysisManager::FeedforwardGains AnalysisManager::CalculateFeedforward() {
WPI_INFO(m_logger, "{}", "Calculating Gains");
// Calculate feedforward gains from the data.
const auto& ff = sysid::CalculateFeedforwardGains(GetFilteredData(), m_type);
FeedforwardGains ffGains = {ff, m_trackWidth};
const auto& ff = sysid::CalculateFeedforwardGains(
GetFilteredData(), m_data.mechanismType, false);
FeedforwardGains ffGains = {ff};
const auto& Ks = ff.coeffs[0];
const auto& Kv = ff.coeffs[1];
@@ -542,27 +220,20 @@ sysid::FeedbackGains AnalysisManager::CalculateFeedback(
if (m_settings.type == FeedbackControllerLoopType::kPosition) {
fb = sysid::CalculatePositionFeedbackGains(
m_settings.preset, m_settings.lqr, Kv, Ka,
m_settings.convertGainsToEncTicks
? m_settings.gearing * m_settings.cpr * m_factor
: 1);
m_settings.convertGainsToEncTicks ? m_settings.gearing * m_settings.cpr
: 1);
} else {
fb = sysid::CalculateVelocityFeedbackGains(
m_settings.preset, m_settings.lqr, Kv, Ka,
m_settings.convertGainsToEncTicks
? m_settings.gearing * m_settings.cpr * m_factor
: 1);
m_settings.convertGainsToEncTicks ? m_settings.gearing * m_settings.cpr
: 1);
}
return fb;
}
void AnalysisManager::OverrideUnits(std::string_view unit,
double unitsPerRotation) {
m_unit = unit;
m_factor = unitsPerRotation;
void AnalysisManager::OverrideUnits(std::string_view unit) {
m_data.distanceUnit = unit;
}
void AnalysisManager::ResetUnitsFromJSON() {
m_unit = m_json.at("units").get<std::string>();
m_factor = m_json.at("unitsPerRotation").get<double>();
}
void AnalysisManager::ResetUnitsFromJSON() {}

6
sysid/src/main/native/cpp/analysis/AnalysisType.cpp
View File

@@ -7,12 +7,6 @@
using namespace sysid;
AnalysisType sysid::analysis::FromName(std::string_view name) {
if (name == "Drivetrain") {
return sysid::analysis::kDrivetrain;
}
if (name == "Drivetrain (Angular)") {
return sysid::analysis::kDrivetrainAngular;
}
if (name == "Elevator") {
return sysid::analysis::kElevator;
}

51
sysid/src/main/native/cpp/analysis/FilteringUtils.cpp
View File

@@ -153,32 +153,24 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
minStepTime = std::min(data->at(0).timestamp - firstTimestamp, minStepTime);
// If step duration hasn't been set yet, calculate a default (find the entry
// before the acceleration first hits zero)
if (settings->stepTestDuration <= minStepTime) {
// Get noise floor
const double accelNoiseFloor = GetNoiseFloor(
*data, kNoiseMeanWindow, [](auto&& pt) { return pt.acceleration; });
// Find latest element with nonzero acceleration
auto endIt = std::find_if(
data->rbegin(), data->rend(), [&](const PreparedData& entry) {
return std::abs(entry.acceleration) > accelNoiseFloor;
});
// 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->rend()) {
// Calculate default duration
settings->stepTestDuration = std::min(
endIt->timestamp - data->front().timestamp + minStepTime + 1_s,
maxStepTime);
} else {
settings->stepTestDuration = maxStepTime;
}
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 + minStepTime >
return entry.timestamp - data->front().timestamp >
settings->stepTestDuration;
});
@@ -186,6 +178,7 @@ sysid::TrimStepVoltageData(std::vector<PreparedData>* data,
if (maxIt != data->end()) {
data->erase(maxIt, data->end());
}
return std::make_tuple(minStepTime, positionDelay, velocityDelay);
}
@@ -204,6 +197,16 @@ double sysid::GetNoiseFloor(
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;
@@ -301,7 +304,7 @@ static units::second_t GetMaxStepTime(
auto key = it.first();
auto& dataset = it.getValue();
if (IsRaw(key) && wpi::contains(key, "fast")) {
if (IsRaw(key) && wpi::contains(key, "dynamic")) {
auto duration = dataset.back().timestamp - dataset.front().timestamp;
if (duration > maxStepTime) {
maxStepTime = duration;
@@ -327,7 +330,7 @@ void sysid::InitialTrimAndFilter(
for (auto& it : preparedData) {
auto key = it.first();
auto& dataset = it.getValue();
if (wpi::contains(key, "slow")) {
if (wpi::contains(key, "quasistatic")) {
settings->motionThreshold =
std::min(settings->motionThreshold,
GetNoiseFloor(dataset, kNoiseMeanWindow,
@@ -342,7 +345,7 @@ void sysid::InitialTrimAndFilter(
// Trim quasistatic test data to remove all points where voltage is zero or
// velocity < motion threshold.
if (wpi::contains(key, "slow")) {
if (wpi::contains(key, "quasistatic")) {
dataset.erase(std::remove_if(dataset.begin(), dataset.end(),
[&](const auto& pt) {
return std::abs(pt.voltage) <= 0 ||
@@ -366,7 +369,7 @@ void sysid::InitialTrimAndFilter(
PrepareMechData(&dataset, unit);
// Trims filtered Dynamic Test Data
if (IsFiltered(key) && wpi::contains(key, "fast")) {
if (IsFiltered(key) && wpi::contains(key, "dynamic")) {
// Get the filtered dataset name
auto filteredKey = RemoveStr(key, "raw-");

164
sysid/src/main/native/cpp/analysis/JSONConverter.cpp
View File

@@ -1,164 +0,0 @@
// 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/JSONConverter.h"
#include <stdexcept>
#include <string>
#include <fmt/core.h>
#include <fmt/format.h>
#include <wpi/Logger.h>
#include <wpi/MemoryBuffer.h>
#include <wpi/fmt/raw_ostream.h>
#include <wpi/json.h>
#include <wpi/raw_ostream.h>
#include "sysid/Util.h"
#include "sysid/analysis/AnalysisManager.h"
#include "sysid/analysis/AnalysisType.h"
// Sizes of the arrays for new sysid data.
inline constexpr size_t kDrivetrainSize = 9;
inline constexpr size_t kGeneralSize = 4;
// Indices for the old data.
inline constexpr size_t kTimestampCol = 0;
inline constexpr size_t kLVoltsCol = 3;
inline constexpr size_t kRVoltsCol = 4;
inline constexpr size_t kLPosCol = 5;
inline constexpr size_t kRPosCol = 6;
inline constexpr size_t kLVelCol = 7;
inline constexpr size_t kRVelCol = 8;
static wpi::json GetJSON(std::string_view path, wpi::Logger& logger) {
std::error_code ec;
std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
wpi::MemoryBuffer::GetFile(path, ec);
if (fileBuffer == nullptr || ec) {
throw std::runtime_error(fmt::format("Unable to read: {}", path));
}
wpi::json json = wpi::json::parse(fileBuffer->GetCharBuffer());
WPI_INFO(logger, "Read frc-characterization JSON from {}", path);
return json;
}
std::string sysid::ConvertJSON(std::string_view path, wpi::Logger& logger) {
wpi::json ojson = GetJSON(path, logger);
auto type = sysid::analysis::FromName(ojson.at("test").get<std::string>());
auto factor = ojson.at("unitsPerRotation").get<double>();
auto unit = ojson.at("units").get<std::string>();
wpi::json json;
for (auto&& key : AnalysisManager::kJsonDataKeys) {
if (type == analysis::kDrivetrain) {
// Get the old data; create a vector for the new data; reserve the
// appropriate size for the new data.
auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
std::vector<std::array<double, kDrivetrainSize>> data;
data.reserve(odata.size());
// Transfer the data.
for (auto&& pt : odata) {
data.push_back(std::array<double, kDrivetrainSize>{
pt[kTimestampCol], pt[kLVoltsCol], pt[kRVoltsCol], pt[kLPosCol],
pt[kRPosCol], pt[kLVelCol], pt[kRVelCol], 0.0, 0.0});
}
json[key] = data;
} else {
// Get the old data; create a vector for the new data; reserve the
// appropriate size for the new data.
auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
std::vector<std::array<double, kGeneralSize>> data;
data.reserve(odata.size());
// Transfer the data.
for (auto&& pt : odata) {
data.push_back(std::array<double, kGeneralSize>{
pt[kTimestampCol], pt[kLVoltsCol], pt[kLPosCol], pt[kLVelCol]});
}
json[key] = data;
}
}
json["units"] = unit;
json["unitsPerRotation"] = factor;
json["test"] = type.name;
json["sysid"] = true;
// Write the new file with "_new" appended to it.
path.remove_suffix(std::string_view{".json"}.size());
std::string loc = fmt::format("{}_new.json", path);
sysid::SaveFile(json.dump(2), std::filesystem::path{loc});
WPI_INFO(logger, "Wrote new JSON to: {}", loc);
return loc;
}
std::string sysid::ToCSV(std::string_view path, wpi::Logger& logger) {
wpi::json json = GetJSON(path, logger);
auto type = sysid::analysis::FromName(json.at("test").get<std::string>());
auto factor = json.at("unitsPerRotation").get<double>();
auto unit = json.at("units").get<std::string>();
std::string_view abbreviation = GetAbbreviation(unit);
std::error_code ec;
// Naming: {sysid-json-name}(Test, Units).csv
path.remove_suffix(std::string_view{".json"}.size());
std::string loc = fmt::format("{} ({}, {}).csv", path, type.name, unit);
wpi::raw_fd_ostream outputFile{loc, ec};
if (ec) {
throw std::runtime_error("Unable to write to: " + loc);
}
fmt::print(outputFile, "Timestamp (s),Test,");
if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
fmt::print(
outputFile,
"Left Volts (V),Right Volts (V),Left Position ({0}),Right "
"Position ({0}),Left Velocity ({0}/s),Right Velocity ({0}/s),Gyro "
"Position (deg),Gyro Rate (deg/s)\n",
abbreviation);
} else {
fmt::print(outputFile, "Volts (V),Position({0}),Velocity ({0}/s)\n",
abbreviation);
}
outputFile << "\n";
for (auto&& key : AnalysisManager::kJsonDataKeys) {
if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
auto tempData =
json.at(key).get<std::vector<std::array<double, kDrivetrainSize>>>();
for (auto&& pt : tempData) {
fmt::print(outputFile, "{},{},{},{},{},{},{},{},{},{}\n",
pt[0], // Timestamp
key, // Test
pt[1], pt[2], // Left and Right Voltages
pt[3] * factor, pt[4] * factor, // Left and Right Positions
pt[5] * factor, pt[6] * factor, // Left and Right Velocity
pt[7], pt[8] // Gyro Position and Velocity
);
}
} else {
auto tempData =
json.at(key).get<std::vector<std::array<double, kGeneralSize>>>();
for (auto&& pt : tempData) {
fmt::print(outputFile, "{},{},{},{},{}\n",
pt[0], // Timestamp,
key, // Test
pt[1], // Voltage
pt[2] * factor, // Position
pt[3] * factor // Velocity
);
}
}
}
outputFile.flush();
WPI_INFO(logger, "Wrote CSV to: {}", loc);
return loc;
}