[sysid] Add SysId (wpilibsuite#5672)

The source is copied from this commit:
wpilibsuite/sysid@625ff04.

CMakeLists.txt

@@ -293,6 +293,7 @@ if (WITH_GUI)
     add_subdirectory(wpigui)
     add_subdirectory(glass)
     add_subdirectory(outlineviewer)
+    add_subdirectory(sysid)
     if (LIBSSH_FOUND)
         add_subdirectory(roborioteamnumbersetter)
         add_subdirectory(datalogtool)

settings.gradle

@@ -40,6 +40,7 @@ include 'glass'
 include 'outlineviewer'
 include 'roborioteamnumbersetter'
 include 'datalogtool'
+include 'sysid'
 include 'simulation:halsim_ds_socket'
 include 'simulation:halsim_gui'
 include 'simulation:halsim_ws_core'

sysid/.styleguide

@@ -0,0 +1,35 @@
+cppHeaderFileInclude {
+  \.h$
+  \.inc$
+  \.inl$
+}
+
+cppSrcFileInclude {
+  \.cpp$
+}
+
+generatedFileExclude {
+  src/main/native/resources/
+  src/main/native/win/sysid.ico
+  src/main/native/mac/sysid.icns
+}
+
+repoRootNameOverride {
+  sysid
+}
+
+includeOtherLibs {
+  ^GLFW
+  ^fmt/
+  ^frc/
+  ^glass/
+  ^gtest/
+  ^imgui
+  ^implot\.h$
+  ^networktables/
+  ^portable-file-dialogs\.h$
+  ^ntcore
+  ^units/
+  ^wpi/
+  ^wpigui
+}

sysid/CMakeLists.txt

@@ -0,0 +1,41 @@
+project(sysid)
+
+include(CompileWarnings)
+include(GenResources)
+include(LinkMacOSGUI)
+include(AddTest)
+
+configure_file(src/main/generate/WPILibVersion.cpp.in WPILibVersion.cpp)
+generate_resources(src/main/native/resources generated/main/cpp SYSID sysid sysid_resources_src)
+
+file(GLOB_RECURSE sysid_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
+
+if (WIN32)
+    set(sysid_rc src/main/native/win/sysid.rc)
+elseif(APPLE)
+    set(MACOSX_BUNDLE_ICON_FILE sysid.icns)
+    set(APP_ICON_MACOSX src/main/native/mac/sysid.icns)
+    set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources")
+endif()
+
+add_executable(sysid ${sysid_src} ${sysid_resources_src} ${sysid_rc} ${APP_ICON_MACOSX})
+wpilib_link_macos_gui(sysid)
+wpilib_target_warnings(sysid)
+target_include_directories(sysid PRIVATE src/main/native/include)
+target_link_libraries(sysid wpimath libglassnt libglass)
+
+if (WIN32)
+    set_target_properties(sysid PROPERTIES WIN32_EXECUTABLE YES)
+elseif(APPLE)
+    set_target_properties(sysid PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "SysId")
+endif()
+
+if (WITH_TESTS)
+    wpilib_add_test(sysid src/test/native/cpp)
+    wpilib_link_macos_gui(sysid_test)
+    target_sources(sysid_test PRIVATE ${sysid_src})
+    target_compile_definitions(sysid_test PRIVATE RUNNING_SYSID_TESTS)
+    target_include_directories(sysid_test PRIVATE src/main/native/cpp
+                                                  src/main/native/include)
+    target_link_libraries(sysid_test wpimath libglassnt libglass gtest)
+endif()

sysid/Info.plist

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>CFBundleName</key>
+	<string>SysId</string>
+	<key>CFBundleExecutable</key>
+	<string>sysid</string>
+	<key>CFBundleDisplayName</key>
+	<string>SysId</string>
+	<key>CFBundleIdentifier</key>
+	<string>edu.wpi.first.tools.SysId</string>
+	<key>CFBundleIconFile</key>
+	<string>sysid.icns</string>
+	<key>CFBundlePackageType</key>
+	<string>APPL</string>
+	<key>CFBundleSupportedPlatforms</key>
+	<array>
+		<string>MacOSX</string>
+	</array>
+	<key>CFBundleInfoDictionaryVersion</key>
+	<string>6.0</string>
+	<key>CFBundleShortVersionString</key>
+	<string>2021</string>
+	<key>CFBundleVersion</key>
+	<string>2021</string>
+	<key>LSMinimumSystemVersion</key>
+	<string>10.14</string>
+	<key>NSHighResolutionCapable</key>
+	<true/>
+</dict>
+</plist>

sysid/build.gradle

@@ -0,0 +1,172 @@
+import org.gradle.internal.os.OperatingSystem
+
+if (project.hasProperty('onlylinuxathena')) {
+    return;
+}
+
+description = 'System identification for robot mechanisms'
+
+apply plugin: 'cpp'
+apply plugin: 'google-test-test-suite'
+apply plugin: 'visual-studio'
+apply plugin: 'edu.wpi.first.NativeUtils'
+
+if (OperatingSystem.current().isWindows()) {
+    apply plugin: 'windows-resources'
+}
+
+ext {
+    nativeName = 'sysid'
+}
+
+apply from: "${rootDir}/shared/resources.gradle"
+apply from: "${rootDir}/shared/config.gradle"
+
+def wpilibVersionFileInput = file("src/main/generate/WPILibVersion.cpp.in")
+def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp")
+
+apply from: "${rootDir}/shared/imgui.gradle"
+
+task generateCppVersion() {
+    description = 'Generates the wpilib version class'
+    group = 'WPILib'
+
+    outputs.file wpilibVersionFileOutput
+    inputs.file wpilibVersionFileInput
+
+    if (wpilibVersioning.releaseMode) {
+        outputs.upToDateWhen { false }
+    }
+
+    // We follow a simple set of checks to determine whether we should generate a new version file:
+    // 1. If the release type is not development, we generate a new version file
+    // 2. If there is no generated version number, we generate a new version file
+    // 3. If there is a generated build number, and the release type is development, then we will
+    //    only generate if the publish task is run.
+    doLast {
+        def version = wpilibVersioning.version.get()
+        println "Writing version ${version} to $wpilibVersionFileOutput"
+
+        if (wpilibVersionFileOutput.exists()) {
+            wpilibVersionFileOutput.delete()
+        }
+        def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version)
+        wpilibVersionFileOutput.write(read)
+    }
+}
+
+gradle.taskGraph.addTaskExecutionGraphListener { graph ->
+    def willPublish = graph.hasTask(publish)
+    if (willPublish) {
+        generateCppVersion.outputs.upToDateWhen { false }
+    }
+}
+
+def generateTask = createGenerateResourcesTask('main', 'SYSID', 'sysid', project)
+
+project(':').libraryBuild.dependsOn build
+tasks.withType(CppCompile) {
+    dependsOn generateTask
+    dependsOn generateCppVersion
+}
+
+model {
+    components {
+        // By default, a development executable will be generated. This is to help the case of
+        // testing specific functionality of the library.
+        "${nativeName}"(NativeExecutableSpec) {
+            baseName = 'sysid'
+            sources {
+                cpp {
+                    source {
+                        srcDirs 'src/main/native/cpp', "$buildDir/generated/main/cpp"
+                        include '**/*.cpp'
+                    }
+                    exportedHeaders {
+                        srcDirs 'src/main/native/include'
+                    }
+                }
+                if (OperatingSystem.current().isWindows()) {
+                    rc.source {
+                        srcDirs 'src/main/native/win'
+                        include '*.rc'
+                    }
+                }
+            }
+            binaries.all {
+                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    it.buildable = false
+                    return
+                }
+                lib project: ':glass', library: 'glassnt', linkage: 'static'
+                lib project: ':glass', library: 'glass', linkage: 'static'
+                project(':ntcore').addNtcoreDependency(it, 'static')
+                lib project: ':wpinet', library: 'wpinet', linkage: 'static'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+                lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+                nativeUtils.useRequiredLibrary(it, 'imgui')
+                if (it.targetPlatform.operatingSystem.isWindows()) {
+                    it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
+                    it.linker.args << '/DELAYLOAD:MF.dll' << '/DELAYLOAD:MFReadWrite.dll' << '/DELAYLOAD:MFPlat.dll' << '/delay:nobind'
+                } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+                    it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore'
+                    if (it.buildType.getName() == "release") {
+                        it.linker.args << '-s'
+                    }
+                } else {
+                    it.linker.args << '-lX11'
+                    if (it.targetPlatform.name.startsWith('linuxarm')) {
+                        it.linker.args << '-lGL'
+                    }
+                }
+            }
+        }
+    }
+    testSuites {
+        "${nativeName}Test"(GoogleTestTestSuiteSpec) {
+            for (NativeComponentSpec c : $.components) {
+                if (c.name == nativeName) {
+                    testing c
+                    break
+                }
+            }
+            sources.cpp.source {
+                srcDirs "src/test/native/cpp"
+                include "**/*.cpp"
+            }
+            binaries.all {
+                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    it.buildable = false
+                    return
+                }
+                lib project: ':glass', library: 'glassnt', linkage: 'static'
+                lib project: ':glass', library: 'glass', linkage: 'static'
+                project(':ntcore').addNtcoreDependency(it, 'static')
+                lib project: ':wpinet', library: 'wpinet', linkage: 'static'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+                lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+                nativeUtils.useRequiredLibrary(it, 'imgui')
+                if (it.targetPlatform.operatingSystem.isWindows()) {
+                    it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
+                    it.linker.args << '/DELAYLOAD:MF.dll' << '/DELAYLOAD:MFReadWrite.dll' << '/DELAYLOAD:MFPlat.dll' << '/delay:nobind'
+                } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+                    it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore'
+                    if (it.buildType.getName() == "release") {
+                        it.linker.args << '-s'
+                    }
+                } else {
+                    it.linker.args << '-lX11'
+                    if (it.targetPlatform.name.startsWith('linuxarm')) {
+                        it.linker.args << '-lGL'
+                    }
+                }
+                nativeUtils.useRequiredLibrary(it, "googletest_static")
+                it.cppCompiler.define("RUNNING_SYSID_TESTS")
+            }
+        }
+    }
+}
+
+apply from: 'publish.gradle'

sysid/docs/arm-ols-with-angle-offset.md

@@ -0,0 +1,66 @@
+# Arm OLS with angle offset
+
+If the arm encoder doesn't read zero degrees when the arm is horizontal, the fit
+for `Kg` will be wrong. An angle offset should be added to the model like so.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle + offset)
+```
+Use a trig identity to split the cosine into two terms.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka (cos(angle) cos(offset) - sin(angle) sin(offset))
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) cos(offset) + Kg/Ka sin(angle) sin(offset)
+```
+Reorder multiplicands so the offset trig is absorbed by the OLS terms.
+```
+dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(offset) cos(angle) + Kg/Ka sin(offset) sin(angle)
+```
+
+## OLS
+
+Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`.
+```
+dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle)
+```
+
+### Ks, Kv, Ka
+
+Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`.
+```
+Ks = -γ/β
+Kv = -α/β
+Ka = 1/β
+```
+
+### Kg
+
+Take the sum of squares of the OLS terms containing the angle offset. The angle
+offset trig functions will form a trig identity that cancels out. Then, just
+solve for `Kg`.
+```
+δ²+ε² = (-Kg/Ka cos(offset))² + (Kg/Ka sin(offset))²
+δ²+ε² = (-Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
+δ²+ε² = (Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset)
+δ²+ε² = (Kg/Ka)² (cos²(offset) + sin²(offset))
+δ²+ε² = (Kg/Ka)² (1)
+δ²+ε² = (Kg/Ka)²
+√(δ²+ε²) = Kg/Ka
+√(δ²+ε²) = Kg β
+Kg = √(δ²+ε²)/β
+```
+
+As a sanity check, when the offset is zero, ε is zero and the equation for
+`Kg` simplifies to -δ/β, the equation previously used by SysId.
+
+### Angle offset
+
+Divide ε by δ, combine the trig functions into `tan(offset)`, then use `atan2()`
+to preserve the angle quadrant. Maintaining the proper negative signs in the
+numerator and denominator are important for obtaining the correct result.
+```
+δ = -Kg/Ka cos(offset)
+ε = Kg/Ka sin(offset)
+sin(offset)/-cos(offset) = ε/δ
+sin(offset)/cos(offset) = ε/-δ
+tan(offset) = ε/-δ
+offset = atan2(ε, -δ)
+```