Merge branch 'main' into 2027

MavenArtifacts.md

@@ -14,54 +14,27 @@ The development repository is where development releases of every commit to [mai
 ## Artifact classifiers
 We provide two base types of artifacts.
 
-The first types are Java artifacts. These are usually published as `jar` files. Usually, the actual jar file is published with no classifier. The sources are published with the `-sources` classifier, and the javadocs are published with the `-javadoc` classifier.
-
-The second types are native artifacts. These are usually published as `zip` files (except for the `JNI` artifact types, which are `jar` files. See below for information on this). The `-sources` and `-headers` classifiers contain the sources and headers respectively for the library. Each artifact also contains a classifier for each platform we publish. This platform is in the format `{os}{arch}`. The platform artifact only contains the binaries for a specific platform. In addition, we provide a `-all` classifier. This classifier combines all of the platform artifacts into a single artifact. This is useful for tools that cannot determine what version to use during builds. However, we recommend using the platform specific classifier when possible. Note that the binary artifacts never contain the headers, you always need the `-headers` classifier to get those.
-
-## Artifact Names
-
-WPILib builds four different types of artifacts.
-
-##### C++ Only Libraries
-When we publish C++ only libraries, they are published with the base artifact name as their artifact name, with a `-cpp` extension. All dependencies for the library are linked as shared libraries to the binary.
-
-
-Example:
-```
-edu.wpi.first.wpilibc:wpilibc-cpp:version:classifier@zip
-```
-
-#### Java Only Libraries
-When we publish Java only libraries, they are published with the base artifact name as their artifact name, with a `-java` extension.
+The first types are Java artifacts. These are usually published as `jar` files. Usually, the actual jar file is published with no classifier. The sources are published with the `-sources` classifier, and the javadocs are published with the `-javadoc` classifier. These artifacts are published with the base artifact name as their artifact ID, with a `-java` extension.
 
 Example:
 ```
 edu.wpi.first.wpilibj:wpilibj-java:version
 ```
 
-#### C++/Java Libraries without JNI
-For libraries that are both C++ and Java, but without a JNI component, the C++ component is published with the `basename-cpp` artifact name, and the Java component is published with the `basename-java` artifact name.
+The second types are native artifacts. These are usually published as `zip` files. The `-sources` and `-headers` classifiers contain the sources and headers respectively for the library. Each artifact also contains a classifier for each platform we publish. This platform is in the format `{os}{arch}`. The full list of supported platforms can be found in [native-utils](https://github.com/wpilibsuite/native-utils/blob/main/src/main/java/edu/wpi/first/nativeutils/WPINativeUtilsExtension.java#L94). If the library is built statically, it will have `static` appended to the classifier. Additionally, if the library was built in debug mode, `debug` will be appended to the classifier. The platform artifact only contains the binaries for a specific platform. Note that the binary artifacts never contain the headers, you always need the `-headers` classifier to get those.
+
+If the library is Java and C++ and has a JNI component, the native artifact will have a shared library containing JNI entrypoints alongside the C++ shared library. This JNI shared library will have a `jni` suffix in the file name.
+
+Native artifacts are published with the base artifact name as their artifact ID, with a `-cpp` extension.
 
 Example:
 ```
-edu.wpi.first.wpiutil:wpiutil-cpp:version:classifier@zip (C++)
-edu.wpi.first.wpiutil:wpiutil-java:version (Java)
-```
-
-#### C++/Java Libraries with JNI
-For libraries that are both C++ and Java with a JNI component there are three different artifact names. For Java, the component is published as `basename-java`. For C++, the `basename-cpp` artifact contains the C++ artifacts with all dependencies linked as shared libraries to the binary. These binaries DO contain the JNI entry points. The `basename-jni` artifact contains identical C++ binaries to the `-cpp` artifact, however all of its dependencies are statically linked, and only the JNI and C entry points are exported.
-
-The `-jni` artifact should only be used in cases where you want to create a self contained Java application where the native artifacts are embedded in the jar. Note in an extraction scenario, extending off of the library is never supported, which is why the C++ entry points are not exposed. The name of the library is randomly generated during extraction. For pretty much all cases, and if you ever want to extend from a native library, you should use the `-cpp` artifacts. GradleRIO uses the `-cpp` artifacts for all platforms, even desktop, for this reason.
-
-Example:
-```
-edu.wpi.first.ntcore:ntcore-cpp:version:classifier@zip (C++)
-edu.wpi.first.ntcore:ntcore-jni:version:classifier (JNI jar library)
-edu.wpi.first.ntcore:ntcore-java:version (Java)
+edu.wpi.first.wpimath:wpimath-cpp:version:classifier@zip
+edu.wpi.first.wpimath:wpimath-cpp:version:windowsx86-64staticdebug@zip
 ```
 
 ## Provided Artifacts
-This repository provides the following artifacts. Below each artifact is its dependencies. Note if ever using the `-jni` artifacts, no dependencies are needed for native binaries.
+This repository provides the following artifacts. Below each artifact is its dependencies.
 
 For C++, if building with static dependencies, the listed order should be the link order in your linker.
 

apriltag/build.gradle

@@ -103,14 +103,8 @@ model {
                 return
             }
             it.cppCompiler.define 'WPILIB_EXPORTS'
-
-            if (it.component.name == "${nativeName}JNI") {
-                lib project: ':wpimath', library: 'wpimath', linkage: 'static'
-                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
-            } else {
-                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
-                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-            }
+            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
         }
     }
     tasks {

cscore/.styleguide

@@ -38,8 +38,10 @@ includeOtherLibs {
   ^fmt/
   ^gtest/
   ^opencv2/
+  ^imgui
   ^support/
   ^tcpsockets/
   ^wpi/
+  ^wpigui
   ^wpinet/
 }

cscore/BUILD.bazel

@@ -48,6 +48,7 @@ objc_library(
         "Foundation",
         "CoreMedia",
         "CoreVideo",
+        "IOKit",
     ],
     tags = ["manual"],
     deps = [

cscore/CMakeLists.txt

@@ -21,7 +21,7 @@ if(APPLE)
         cscore
         PROPERTIES
             LINK_FLAGS
-                "-framework CoreFoundation -framework AVFoundation -framework Foundation -framework CoreMedia -framework CoreVideo"
+                "-framework CoreFoundation -framework AVFoundation -framework Foundation -framework CoreMedia -framework CoreVideo -framework IOKit"
     )
 elseif(MSVC)
     target_sources(cscore PRIVATE ${cscore_windows_src})

cscore/build.gradle

@@ -60,13 +60,8 @@ model {
             if (!it.buildable || !(it instanceof NativeBinarySpec)) {
                 return
             }
-            if (it.component.name == "${nativeName}JNI") {
-                lib project: ':wpinet', library: 'wpinet', linkage: 'static'
-                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
-            } else {
-                lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
-                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-            }
+            lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
+            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
         }
     }
 }

cscore/examples/usbviewer/usbviewer.cpp

@@ -3,14 +3,17 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <atomic>
+#include <memory>
 #include <thread>
+#include <utility>
 #include <vector>
 
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <opencv2/core/core.hpp>
+#include <opencv2/core/mat.hpp>
 #include <opencv2/imgproc.hpp>
+#include <wpi/mutex.h>
 #include <wpi/print.h>
 #include <wpi/spinlock.h>
 #include <wpigui.h>
@@ -21,10 +24,10 @@
 namespace gui = wpi::gui;
 
 int main() {
-  std::atomic<cv::Mat*> latestFrame{nullptr};
-  std::vector<cv::Mat*> sharedFreeList;
-  wpi::spinlock sharedFreeListMutex;
-  std::vector<cv::Mat*> sourceFreeList;
+  wpi::spinlock latestFrameMutex;
+  std::unique_ptr<cv::Mat> latestFrame;
+  wpi::mutex freeListMutex;
+  std::vector<std::unique_ptr<cv::Mat>> freeList;
   std::atomic<bool> stopCamera{false};
 
   cs::UsbCamera camera{"usbcam", 0};
@@ -42,36 +45,31 @@ int main() {
         continue;
       }
 
-      // get or create a mat, prefer sourceFreeList over sharedFreeList
-      cv::Mat* out;
-      if (!sourceFreeList.empty()) {
-        out = sourceFreeList.back();
-        sourceFreeList.pop_back();
-      } else {
-        {
-          std::scoped_lock lock(sharedFreeListMutex);
-          for (auto mat : sharedFreeList) {
-            sourceFreeList.emplace_back(mat);
-          }
-          sharedFreeList.clear();
-        }
-        if (!sourceFreeList.empty()) {
-          out = sourceFreeList.back();
-          sourceFreeList.pop_back();
+      // get or create a mat
+      std::unique_ptr<cv::Mat> out;
+      {
+        std::scoped_lock lock{freeListMutex};
+        if (!freeList.empty()) {
+          out = std::move(freeList.back());
+          freeList.pop_back();
         } else {
-          out = new cv::Mat;
+          out = std::make_unique<cv::Mat>();
         }
       }
 
       // convert to RGBA
       cv::cvtColor(frame, *out, cv::COLOR_BGR2RGBA);
 
-      // make available
-      auto prev = latestFrame.exchange(out);
+      {
+        // make available
+        std::scoped_lock lock{latestFrameMutex};
+        latestFrame.swap(out);
+      }
 
-      // put prev on free list
-      if (prev) {
-        sourceFreeList.emplace_back(prev);
+      // put the previous frame on free list
+      if (out) {
+        std::scoped_lock lock{freeListMutex};
+        freeList.emplace_back(std::move(out));
       }
     }
   });
@@ -80,7 +78,11 @@ int main() {
   gui::Initialize("Hello World", 1024, 768);
   gui::Texture tex;
   gui::AddEarlyExecute([&] {
-    auto frame = latestFrame.exchange(nullptr);
+    std::unique_ptr<cv::Mat> frame;
+    {
+      std::scoped_lock lock{latestFrameMutex};
+      latestFrame.swap(frame);
+    }
     if (frame) {
       // create or update texture
       if (!tex || frame->cols != tex.GetWidth() ||
@@ -90,9 +92,10 @@ int main() {
       } else {
         tex.Update(frame->data);
       }
-      // put back on shared freelist
-      std::scoped_lock lock(sharedFreeListMutex);
-      sharedFreeList.emplace_back(frame);
+      {
+        std::scoped_lock lock{freeListMutex};
+        freeList.emplace_back(std::move(frame));
+      }
     }
 
     ImGui::SetNextWindowSize(ImVec2(640, 480), ImGuiCond_FirstUseEver);

cscore/src/main/native/objcpp/UsbCameraImpl.h

@@ -12,6 +12,8 @@
 #include <string>
 #include <optional>
 
+#include <wpi/StringMap.h>
+
 #include "SourceImpl.h"
 
 namespace cs {
@@ -88,8 +90,34 @@ class UsbCameraImpl : public SourceImpl {
 
   UsbCameraImplObjc* cppGetObjc() { return m_objc; }
 
+  int CreatePropertyPublic(std::string_view name, std::function<std::unique_ptr<PropertyImpl>()> newFunc) {
+    return CreateProperty(name, newFunc);
+  }
+
+  PropertyImpl* GetPropertyPublic(int property) {
+    return GetProperty(property);
+  }
+
+  void NotifyPropertyCreatedPublic(int propIndex, PropertyImpl& prop) {
+    NotifyPropertyCreated(propIndex, prop);
+  }
+
+  void UpdatePropertyValuePublic(int property, bool setString, int value, std::string_view valueStr) {
+    UpdatePropertyValue(property, setString, value, valueStr);
+  }
+
+  wpi::mutex& GetMutex() { return m_mutex; }
+
+  // Property cache accessors
+  wpi::StringMap<uint32_t>& GetPropertyCache() { return m_propertyCache; }
+  wpi::StringMap<uint32_t>& GetPropertyAutoCache() { return m_propertyAutoCache; }
+
  private:
   UsbCameraImplObjc* m_objc;
   std::vector<CameraModeStore> m_platformModes;
+  
+  // Property caches
+  wpi::StringMap<uint32_t> m_propertyCache;
+  wpi::StringMap<uint32_t> m_propertyAutoCache;
 };
 }  // namespace cs

cscore/src/main/native/objcpp/UsbCameraImplObjc.h

@@ -5,11 +5,39 @@
 #pragma once
 
 #import <AVFoundation/AVFoundation.h>
-#import "UsbCameraDelegate.h"
+
 #include <memory>
 #include <string_view>
+
+#import "UsbCameraDelegate.h"
+#import "UvcControlImpl.h"
+
 #include "cscore_cpp.h"
 
+// Quirk: exposure auto is 3 for on, 1 for off
+#define kPropertyAutoExposureOn 3
+#define kPropertyAutoExposureOff 1
+
+// Property names
+#define kPropertyBrightness "brightness"
+#define kPropertyWhiteBalance "white_balance_temperature"
+#define kPropertyExposure "raw_exposure_time_absolute"
+#define kPropertyContrast "raw_contrast"
+#define kPropertySaturation "raw_saturation"
+#define kPropertySharpness "raw_sharpness"
+#define kPropertyGain "gain"
+#define kPropertyGamma "gamma"
+#define kPropertyHue "raw_hue"
+#define kPropertyFocus "focus_absolute"
+#define kPropertyZoom "zoom"
+#define kPropertyBackLightCompensation "backlight_compensation"
+#define kPropertyPowerLineFrequency "power_line_frequency"
+
+// Auto property names
+#define kPropertyAutoExposure "exposure_auto"
+#define kPropertyAutoWhiteBalance "white_balance_automatic"
+#define kPropertyAutoFocus "focus_auto"
+
 namespace cs {
 class UsbCameraImpl;
 }
@@ -30,6 +58,7 @@ class UsbCameraImpl;
 @property(nonatomic) AVCaptureDevice* videoDevice;
 @property(nonatomic) AVCaptureDeviceInput* videoInput;
 @property(nonatomic) UsbCameraDelegate* callback;
+@property(nonatomic) UvcControlImpl* uvcControl;
 @property(nonatomic) AVCaptureVideoDataOutput* videoOutput;
 @property(nonatomic) AVCaptureSession* session;
 
@@ -68,4 +97,8 @@ class UsbCameraImpl;
 - (void)getCameraName:(std::string*)name;
 - (void)setNewCameraPath:(std::string_view*)path;
 
+- (void)deviceCacheProperties;
+- (void)cacheProperty:(uint32_t)propID withName:(NSString *)name;
+- (void)cacheAutoProperty:(uint32_t)propID withName:(NSString *)baseName;
+
 @end

cscore/src/main/native/objcpp/UsbCameraImplObjc.mm

@@ -2,12 +2,14 @@
 // 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.
 
-#import "UsbCameraImplObjc.h"
-#include "UsbCameraImpl.h"
+#include <wpi/SmallString.h>
 
 #pragma GCC diagnostic ignored "-Wunused-parameter"
+#import "UsbCameraImplObjc.h"
+
 #include "Notifier.h"
 #include "Log.h"
+#include "UsbCameraImpl.h"
 
 template <typename S, typename... Args>
 inline void NamedLog(UsbCameraImplObjc* objc, unsigned int level,
@@ -104,44 +106,300 @@ using namespace cs;
                name:AVCaptureDeviceWasDisconnectedNotification
              object:nil];
     [self deviceConnect];
+    [self deviceCacheProperties];
   });
 }
 
+- (BOOL)getEnabledWithProperty:(int)property withValue:(int)value {
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    return false;
+  }
+
+  // There is room for quirk handling improvement here, but I will leave it
+  // for now.
+  if (property == sharedThis->GetPropertyIndex(kPropertyAutoExposure)) {
+    return value == kPropertyAutoExposureOn;
+  }
+
+  return value != 0;
+}
+
+
+- (int)clampToPercent:(int)value {
+  if (value < 0) {
+    return 0;
+  }
+  if (value > 100) {
+    return 100;
+  }
+  return value;
+}
+
+- (int)percentageToRaw:(int)propID percentage:(int)percentage min:(int)min max:(int)max {
+  if (min == max) {
+    return min;
+  }
+
+  return min + (max - min) * percentage / 100;
+}
+
+- (BOOL)isPercentageProperty:(int)propID {
+    return propID == CAPPROPID_BRIGHTNESS ||
+           propID == CAPPROPID_CONTRAST ||
+           propID == CAPPROPID_SATURATION ||
+           propID == CAPPROPID_HUE ||
+           propID == CAPPROPID_SHARPNESS ||
+           propID == CAPPROPID_GAIN;
+}
+
 // Property functions
 - (void)setProperty:(int)property
           withValue:(int)value
              status:(CS_Status*)status {
+    auto sharedThis = self.cppImpl.lock();
+    if (!sharedThis) {
+        *status = CS_INVALID_HANDLE;
+        return;
+    }
+    
+    // Make sure properties are cached
+    if (!self.propertiesCached) {
+        [self deviceCacheProperties];
+    }
+    
+    // Get the property name from the property index
+    wpi::SmallString<128> nameBuf;
+    std::string_view propName = sharedThis->GetPropertyName(property, nameBuf, status);
+    if (*status != 0) {
+        OBJCERROR("Failed to get property name for index {}", property);
+        return;
+    }
+    
+    std::string nameStr(propName);
+    
+    // Check if it's an auto property
+    auto& propertyAutoCache = sharedThis->GetPropertyAutoCache();
+    auto autoIt = propertyAutoCache.find(nameStr);
+    if (autoIt != propertyAutoCache.end()) {
+        uint32_t propID = autoIt->second;
+        bool enabled = [self getEnabledWithProperty:property withValue:value];
+        dispatch_async_and_wait(self.sessionQueue, ^{
+            if (self.uvcControl == nil) {
+                *status = CS_INVALID_PROPERTY;
+                return;
+            }
+            
+            if (![self.uvcControl setAutoProperty:propID enabled:enabled status:status]) {
+                OBJCERROR("Failed to set auto property {} to {}",
+                        nameStr, enabled);
+                return;
+            }
+            
+            // Update property value
+            sharedThis->UpdatePropertyValuePublic(property, false, value, {});
+        });
+        return;
+    }
+    
+    // Handle regular property
+    auto& propertyCache = sharedThis->GetPropertyCache();
+    auto it = propertyCache.find(nameStr);
+    if (it == propertyCache.end()) {
+        OBJCERROR("Property not found in cache: {}", nameStr);
+        *status = CS_INVALID_PROPERTY;
+        return;
+    }
+    
+    uint32_t propID = it->second;
+    
+    dispatch_async_and_wait(self.sessionQueue, ^{
+        if (self.uvcControl == nil) {
+            *status = CS_INVALID_PROPERTY;
+            return;
+        }
+        
+        // Get the property implementation to access its limits
+        const PropertyImpl* prop = sharedThis->GetPropertyPublic(property);
+        if (!prop) {
+            *status = CS_INVALID_PROPERTY;
+            return;
+        }
+        
+
+        int32_t realValue = value;
+        if ([self isPercentageProperty:propID]) {
+            // Clamp to 0-100
+            realValue = [self clampToPercent:realValue];
+
+            // Scale to min/max
+            realValue = [self percentageToRaw:propID percentage:realValue min:prop->minimum max:prop->maximum];
+        }
+
+        if (![self.uvcControl setProperty:propID withValue:realValue status:status]) {
+            OBJCERROR("Failed to set property {} to value {}", nameStr, realValue);
+            return;
+        }
+        
+        // Update property value in the container
+        sharedThis->UpdatePropertyValuePublic(property, false, value, {});
+    });
 }
+
 - (void)setStringProperty:(int)property
                 withValue:(std::string_view*)value
                    status:(CS_Status*)status {
+  *status = CS_INVALID_PROPERTY;
+  return;
 }
 
 // Standard common camera properties
 - (void)setBrightness:(int)brightness status:(CS_Status*)status {
-  *status = CS_INVALID_PROPERTY;
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  
+  // Make sure properties are cached
+  if (!self.propertiesCached) {
+    [self deviceCacheProperties];
+  }
+  
+  // Get the property index and set it
+  int prop = sharedThis->GetPropertyIndex(kPropertyBrightness);
+  sharedThis->SetProperty(prop, brightness, status);
 }
+
 - (int)getBrightness:(CS_Status*)status {
-  *status = CS_INVALID_PROPERTY;
-  return 0;
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    *status = CS_INVALID_HANDLE;
+    return 0;
+  }
+  
+  // Make sure properties are cached
+  if (!self.propertiesCached) {
+    [self deviceCacheProperties];
+  }
+  
+  // Get the property index and its value
+  int prop = sharedThis->GetPropertyIndex(kPropertyBrightness);
+  return sharedThis->GetProperty(prop, status);
 }
+
 - (void)setWhiteBalanceAuto:(CS_Status*)status {
-  *status = CS_INVALID_PROPERTY;
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  
+  // Make sure properties are cached
+  if (!self.propertiesCached) {
+    [self deviceCacheProperties];
+  }
+  
+  int prop = sharedThis->GetPropertyIndex(kPropertyAutoWhiteBalance);
+  sharedThis->SetProperty(prop, 1, status);
 }
+
 - (void)setWhiteBalanceHoldCurrent:(CS_Status*)status {
-  *status = CS_INVALID_PROPERTY;
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  
+  // Make sure properties are cached
+  if (!self.propertiesCached) {
+    [self deviceCacheProperties];
+  }
+  
+  int prop = sharedThis->GetPropertyIndex(kPropertyAutoWhiteBalance);
+  sharedThis->SetProperty(prop, 0, status);
 }
+
 - (void)setWhiteBalanceManual:(int)value status:(CS_Status*)status {
-  *status = CS_INVALID_PROPERTY;
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  
+  // Make sure properties are cached
+  if (!self.propertiesCached) {
+    [self deviceCacheProperties];
+  }
+  
+  // First disable auto white balance
+  int autoProp = sharedThis->GetPropertyIndex(kPropertyAutoWhiteBalance);
+  sharedThis->SetProperty(autoProp, 0, status);
+  if (*status != 0) {
+    return;
+  }
+  
+  // Then set the white balance value
+  int prop = sharedThis->GetPropertyIndex(kPropertyWhiteBalance);
+  sharedThis->SetProperty(prop, value, status);
 }
+
 - (void)setExposureAuto:(CS_Status*)status {
-  *status = CS_INVALID_PROPERTY;
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  
+  // Make sure properties are cached
+  if (!self.propertiesCached) {
+    [self deviceCacheProperties];
+  }
+  
+  // Set the auto exposure property to enabled (1)
+  int prop = sharedThis->GetPropertyIndex(kPropertyAutoExposure);
+  sharedThis->SetProperty(prop, kPropertyAutoExposureOn, status);
 }
+
 - (void)setExposureHoldCurrent:(CS_Status*)status {
-  *status = CS_INVALID_PROPERTY;
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  
+  // Make sure properties are cached
+  if (!self.propertiesCached) {
+    [self deviceCacheProperties];
+  }
+  
+  // Set the auto exposure property to disabled (0)
+  int prop = sharedThis->GetPropertyIndex(kPropertyAutoExposure);
+  sharedThis->SetProperty(prop, kPropertyAutoExposureOff, status);
 }
+
 - (void)setExposureManual:(int)value status:(CS_Status*)status {
-  *status = CS_INVALID_PROPERTY;
+  auto sharedThis = self.cppImpl.lock();
+  if (!sharedThis) {
+    *status = CS_INVALID_HANDLE;
+    return;
+  }
+  
+  // Make sure properties are cached
+  if (!self.propertiesCached) {
+    [self deviceCacheProperties];
+  }
+  
+  // First disable auto exposure
+  int autoProp = sharedThis->GetPropertyIndex(kPropertyAutoExposure);
+  sharedThis->SetProperty(autoProp, kPropertyAutoExposureOff, status);
+  if (*status != 0) {
+    return;
+  }
+  
+  // Then set the exposure value
+  int prop = sharedThis->GetPropertyIndex(kPropertyExposure);
+  sharedThis->SetProperty(prop, value, status);
 }
 
 - (bool)setVideoMode:(const cs::VideoMode&)mode status:(CS_Status*)status {
@@ -295,10 +553,144 @@ using namespace cs;
 
 // All above are called from C++, must always dispatch to loop
 
+// Property caching methods
 - (void)deviceCacheProperties {
-  if (self.session == nil) {
-    return;
-  }
+    if (self.uvcControl == nil) {
+        return;
+    }
+
+    auto sharedThis = self.cppImpl.lock();
+    if (!sharedThis) {
+        OBJCERROR("Cannot cache properties: UsbCameraImpl not available");
+        return;
+    }
+
+    // Cache basic properties
+    [self cacheProperty:CAPPROPID_BRIGHTNESS withName:@kPropertyBrightness];
+    [self cacheProperty:CAPPROPID_WHITEBALANCE withName:@kPropertyWhiteBalance];
+    [self cacheProperty:CAPPROPID_EXPOSURE withName:@kPropertyExposure];
+    [self cacheProperty:CAPPROPID_CONTRAST withName:@kPropertyContrast];
+    [self cacheProperty:CAPPROPID_SATURATION withName:@kPropertySaturation];
+    [self cacheProperty:CAPPROPID_SHARPNESS withName:@kPropertySharpness];
+    [self cacheProperty:CAPPROPID_GAIN withName:@kPropertyGain];
+    [self cacheProperty:CAPPROPID_GAMMA withName:@kPropertyGamma];
+    [self cacheProperty:CAPPROPID_HUE withName:@kPropertyHue];
+    [self cacheProperty:CAPPROPID_FOCUS withName:@kPropertyFocus];
+    [self cacheProperty:CAPPROPID_ZOOM withName:@kPropertyZoom];
+    [self cacheProperty:CAPPROPID_BACKLIGHTCOMP withName:@kPropertyBackLightCompensation];
+    [self cacheProperty:CAPPROPID_POWERLINEFREQ withName:@kPropertyPowerLineFrequency];
+    
+    // Cache auto properties
+    [self cacheAutoProperty:CAPPROPID_EXPOSURE withName:@kPropertyAutoExposure];
+    [self cacheAutoProperty:CAPPROPID_WHITEBALANCE withName:@kPropertyAutoWhiteBalance];
+    [self cacheAutoProperty:CAPPROPID_FOCUS withName:@kPropertyAutoFocus];
+    
+    self.propertiesCached = true;
+}
+
+- (void)cacheProperty:(uint32_t)propID withName:(NSString *)name {
+    auto sharedThis = self.cppImpl.lock();
+    if (!sharedThis) {
+        OBJCERROR("Cannot cache property: UsbCameraImpl not available");
+        return;
+    }
+    
+    if (self.uvcControl == nil) {
+        OBJCWARNING("Cannot cache property {}: UVC control not initialized", [name UTF8String]);
+        return;
+    }
+    
+    // Get property limits
+    int32_t minimum = 0, maximum = 0, defaultValue = 0;
+    int32_t value = defaultValue;
+    CS_Status status;
+    
+    std::string nameStr = std::string([name UTF8String]);
+    
+    // Get the property limits
+    if (![self.uvcControl getPropertyLimits:propID 
+                                      min:&minimum 
+                                      max:&maximum 
+                                 defValue:&defaultValue 
+                                   status:&status]) {
+        OBJCWARNING("Failed to get property limits for {}", nameStr);
+        return;
+    }
+    
+    // Get current value
+    if (![self.uvcControl getProperty:propID withValue:&value status:&status]) {
+        value = defaultValue;
+        OBJCWARNING("Failed to get current value for {}: {}", 
+                  nameStr, value);
+        return;
+    }
+    
+    // Create property
+    auto& propertyCache = sharedThis->GetPropertyCache();
+    propertyCache[nameStr] = propID;
+    
+    // Create the property implementation
+    std::unique_ptr<PropertyImpl> prop;
+    prop = std::make_unique<PropertyImpl>(nameStr);
+    prop->propKind = CS_PROP_INTEGER;
+    prop->value = value;
+    prop->minimum = minimum;
+    prop->maximum = maximum;
+    prop->step = 1;  // Most camera properties use a step of 1
+    prop->defaultValue = defaultValue;
+    
+    // Add the property to the container
+    std::scoped_lock lock(sharedThis->GetMutex());
+    int ndx = sharedThis->CreatePropertyPublic(nameStr, [&] { return std::move(prop); });
+    
+    // Notify that property has been created
+    sharedThis->NotifyPropertyCreatedPublic(ndx, *sharedThis->GetPropertyPublic(ndx));
+}
+
+- (void)cacheAutoProperty:(uint32_t)propID withName:(NSString *)baseName {
+    auto sharedThis = self.cppImpl.lock();
+    if (!sharedThis) {
+        OBJCERROR("Cannot cache auto property: UsbCameraImpl not available");
+        return;
+    }
+    
+    if (self.uvcControl == nil) {
+        OBJCWARNING("Cannot cache auto property {}: UVC control not initialized", [baseName UTF8String]);
+        return;
+    }
+    
+    // Build auto mode property name
+    std::string nameStr = std::string([baseName UTF8String]);
+    
+    // Get current auto mode status
+    bool enabled = false;
+    CS_Status status = 0;
+    
+    if(![self.uvcControl getAutoProperty:propID enabled:&enabled status:&status]) {
+        OBJCWARNING("Failed to get auto property {}", nameStr);
+        return;
+    }
+    
+    // Create property
+    std::unique_ptr<PropertyImpl> prop;
+    prop = std::make_unique<PropertyImpl>(nameStr);
+    prop->propKind = CS_PROP_BOOLEAN;
+    prop->value = enabled ? 1 : 0;
+    prop->minimum = 0;
+    prop->maximum = 1;
+    prop->step = 1;
+    prop->defaultValue = 0;  // Default is manual mode
+    
+    // Add property to container
+    std::scoped_lock lock(sharedThis->GetMutex());
+    int ndx = sharedThis->CreatePropertyPublic(nameStr, [&] { return std::move(prop); });
+    
+    // Notify property created
+    sharedThis->NotifyPropertyCreatedPublic(ndx, *sharedThis->GetPropertyPublic(ndx));
+    
+    // Map property name to ID
+    auto& propertyAutoCache = sharedThis->GetPropertyAutoCache();
+    propertyAutoCache[nameStr] = propID;
 }
 
 static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) {
@@ -459,6 +851,53 @@ static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) {
   self.deviceValid = true;
 }
 
+- (CMTime)findNearestFrameDuration:(int)fps {
+  if (self.currentFormat == nil) {
+    return CMTimeMake(1, fps);
+  }
+
+  NSArray<AVFrameRateRange*>* frameRates = self.currentFormat.videoSupportedFrameRateRanges;
+  if (frameRates.count == 0) {
+    return CMTimeMake(1, fps);
+  }
+
+  // Find the nearest frame duration
+  CMTime nearestDuration = CMTimeMake(1, fps);
+  double minDiff = DBL_MAX;
+
+  for (AVFrameRateRange* range in frameRates) {
+    CMTime minDuration = range.minFrameDuration;
+    CMTime maxDuration = range.maxFrameDuration;
+    
+    // Calculate frame duration for current fps
+    CMTime targetDuration = CMTimeMake(1, fps);
+    
+    // Check if within range
+    if (CMTimeCompare(targetDuration, minDuration) >= 0 && 
+        CMTimeCompare(targetDuration, maxDuration) <= 0) {
+      return targetDuration;
+    }
+    
+    // Calculate difference with min value
+    double minDiffValue = fabs(CMTimeGetSeconds(targetDuration) - CMTimeGetSeconds(minDuration));
+    if (minDiffValue < minDiff) {
+      minDiff = minDiffValue;
+      nearestDuration = minDuration;
+    }
+    
+    // Calculate difference with max value
+    double maxDiffValue = fabs(CMTimeGetSeconds(targetDuration) - CMTimeGetSeconds(maxDuration));
+    if (maxDiffValue < minDiff) {
+      minDiff = maxDiffValue;
+      nearestDuration = maxDuration;
+    }
+  }
+
+  OBJCDEBUG("Nearest fps: {}", nearestDuration.timescale / static_cast<double>(nearestDuration.value));
+  
+  return nearestDuration;
+}
+
 - (bool)deviceStreamOn {
   if (self.streaming) {
     return false;
@@ -466,24 +905,32 @@ static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) {
   if (!self.deviceValid) {
     return false;
   }
-  self.streaming = true;
 
+  if (![self.videoDevice lockForConfiguration:nil]) {
+    OBJCERROR("Failed to lock for configuration");
+    return false;
+  }
+
+  [self.session beginConfiguration];
+
+  if (self.currentFormat != nil) {
+    self.videoDevice.activeFormat = self.currentFormat;
+  }
+
+  if (self.currentFPS != 0) {
+    CMTime frameDuration = [self findNearestFrameDuration:self.currentFPS];
+    self.videoDevice.activeVideoMinFrameDuration = frameDuration;
+    self.videoDevice.activeVideoMaxFrameDuration = frameDuration;
+  }
+
+  [self.session commitConfiguration];
+
+  self.streaming = true;
+  // Start the capture session before device unlock to ensure
+  // the session preset settings are preserved
   [self.session startRunning];
 
-  if ([self.videoDevice lockForConfiguration:nil]) {
-    if (self.currentFormat != nil) {
-      self.videoDevice.activeFormat = self.currentFormat;
-    }
-    if (self.currentFPS != 0) {
-      self.videoDevice.activeVideoMinFrameDuration =
-          CMTimeMake(1, self.currentFPS);
-      self.videoDevice.activeVideoMaxFrameDuration =
-          CMTimeMake(1, self.currentFPS);
-    }
-    [self.videoDevice unlockForConfiguration];
-  } else {
-    OBJCERROR("Failed to lock for configuration");
-  }
+  [self.videoDevice unlockForConfiguration];
 
   return true;
 }
@@ -574,6 +1021,16 @@ static cs::VideoMode::PixelFormat FourCCToPixelFormat(FourCharCode fourcc) {
     goto err;
   }
 
+  CS_Status status;
+  self.uvcControl = [UvcControlImpl createFromAVCaptureDevice:self.videoDevice status:&status];
+  if (self.uvcControl == nil) {
+    OBJCWARNING("Failed to initialize UVC control for camera: {}", status);
+  } else {
+    OBJCINFO("UVC control initialized successfully");
+  }
+  
+  self.uvcControl.cppImpl = self.cppImpl;
+
   self.callback = [[UsbCameraDelegate alloc] init];
   if (self.callback == nil) {
     OBJCWARNING("Creating Camera Callback failed");

cscore/src/main/native/objcpp/UvcControlImpl.h

@@ -0,0 +1,129 @@
+// 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.
+
+#pragma once
+
+#import <AVFoundation/AVFoundation.h>
+#import <IOKit/IOCFPlugIn.h>
+#import <IOKit/usb/IOUSBLib.h>
+
+#include <memory>
+#include <string_view>
+
+#import "UsbCameraDelegate.h"
+
+#include "cscore_cpp.h"
+
+// Status code definition
+#define CS_UVC_STATUS_ERROR -3001
+#define CS_UVC_STATUS_DEVICE_DISCONNECTED -3002
+
+// UVC control selector definitions
+#define UVC_INPUT_TERMINAL_ID 0x01
+
+// Camera terminal control selectors
+#define CT_AE_MODE_CONTROL 0x02
+#define CT_AE_PRIORITY_CONTROL 0x03
+#define CT_EXPOSURE_TIME_ABSOLUTE_CONTROL 0x04
+#define CT_EXPOSURE_TIME_RELATIVE_CONTROL 0x05
+#define CT_FOCUS_ABSOLUTE_CONTROL 0x06
+#define CT_FOCUS_RELATIVE_CONTROL 0x07
+#define CT_FOCUS_AUTO_CONTROL 0x08
+#define CT_ZOOM_ABSOLUTE_CONTROL 0x0B
+#define CT_ZOOM_RELATIVE_CONTROL 0x0C
+
+// Processing unit control selectors
+#define PU_BACKLIGHT_COMPENSATION_CONTROL 0x01
+#define PU_BRIGHTNESS_CONTROL 0x02
+#define PU_CONTRAST_CONTROL 0x03
+#define PU_GAIN_CONTROL 0x04
+#define PU_POWER_LINE_FREQUENCY_CONTROL 0x05
+#define PU_HUE_CONTROL 0x06
+#define PU_SATURATION_CONTROL 0x07
+#define PU_SHARPNESS_CONTROL 0x08
+#define PU_GAMMA_CONTROL 0x09
+#define PU_WHITE_BALANCE_TEMPERATURE_CONTROL 0x0A
+#define PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL 0x0B
+#define PU_WHITE_BALANCE_COMPONENT_CONTROL 0x0C
+#define PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL 0x0D
+#define PU_HUE_AUTO_CONTROL 0x10
+#define PU_CONTRAST_AUTO_CONTROL 0x13
+
+// Camera request error code
+#define VC_REQUEST_ERROR_CODE_CONTROL 0x02
+
+// UVC control interface definitions
+#define UVC_CONTROL_INTERFACE_CLASS 14
+#define UVC_CONTROL_INTERFACE_SUBCLASS 1
+
+// UVC control request types
+#define UVC_SET_CUR 0x01
+#define UVC_GET_CUR 0x81
+#define UVC_GET_MIN 0x82
+#define UVC_GET_MAX 0x83
+#define UVC_GET_RES 0x84
+#define UVC_GET_INFO 0x86
+#define UVC_GET_DEF 0x87
+
+// Camera property ID definitions
+#define CAPPROPID_EXPOSURE 1
+#define CAPPROPID_FOCUS 2
+#define CAPPROPID_ZOOM 3
+#define CAPPROPID_WHITEBALANCE 4
+#define CAPPROPID_GAIN 5
+#define CAPPROPID_BRIGHTNESS 6
+#define CAPPROPID_CONTRAST 7
+#define CAPPROPID_SATURATION 8
+#define CAPPROPID_GAMMA 9
+#define CAPPROPID_HUE 10
+#define CAPPROPID_SHARPNESS 11
+#define CAPPROPID_BACKLIGHTCOMP 12
+#define CAPPROPID_POWERLINEFREQ 13
+#define CAPPROPID_LAST 14
+
+namespace cs {
+class UsbCameraImpl;
+}
+
+@interface UvcControlImpl : NSObject
+
+@property(nonatomic) IOUSBInterfaceInterface190** controlInterface;
+@property(nonatomic) uint32_t processingUnitID;
+@property(nonatomic) std::weak_ptr<cs::UsbCameraImpl> cppImpl;
+
+// Create from AVCaptureDevice
++ (instancetype)createFromAVCaptureDevice:(AVCaptureDevice*)device status:(CS_Status*)status;
+
+// Initialize with USB vendor/product/location
+- (instancetype)initWithVendorId:(uint16_t)vid 
+                      productId:(uint16_t)pid 
+                       location:(uint32_t)location
+                         status:(CS_Status*)status;
+
+- (void)dealloc;
+
+// Basic property control
+- (bool)setProperty:(uint32_t)propID 
+          withValue:(int32_t)value
+             status:(CS_Status*)status;
+- (bool)getProperty:(uint32_t)propID 
+          withValue:(int32_t*)value
+             status:(CS_Status*)status;
+
+// Auto mode control
+- (bool)setAutoProperty:(uint32_t)propID 
+               enabled:(bool)enabled
+                status:(CS_Status*)status;
+- (bool)getAutoProperty:(uint32_t)propID 
+               enabled:(bool*)enabled
+                status:(CS_Status*)status;
+
+// Property range query
+- (bool)getPropertyLimits:(uint32_t)propID
+                     min:(int32_t*)min
+                     max:(int32_t*)max
+                defValue:(int32_t*)defValue
+                  status:(CS_Status*)status;
+
+@end 

cscore/src/main/native/objcpp/UvcControlImpl.mm

@@ -0,0 +1,773 @@
+// 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.
+
+// Copyright (c) 2017 Jason von Nieda, Niels Moseley
+//
+// The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#import <AVFoundation/AVFoundation.h>
+
+#import "UvcControlImpl.h"
+
+#include "Log.h"
+#include "UsbCameraImpl.h"
+
+template <typename S, typename... Args>
+inline void NamedLog(UvcControlImpl* objc, unsigned int level,
+                     const char* file, unsigned int line, const S& format,
+                     Args&&... args) {
+  auto sharedThis = objc.cppImpl.lock();
+  if (!sharedThis) {
+    return;
+  }
+
+  wpi::Logger& logger = sharedThis->objcGetLogger();
+  std::string_view name = sharedThis->GetName();
+
+  if (logger.HasLogger() && level >= logger.min_level()) {
+    cs::NamedLogV(logger, level, file, line, name, format,
+                  fmt::make_format_args(args...));
+  }
+}
+
+#define UVCLOG(level, format, ...) \
+  NamedLog(self, level, __FILE__, __LINE__, \
+           format __VA_OPT__(, ) __VA_ARGS__)
+
+#define UVCERROR(format, ...) \
+  UVCLOG(::wpi::WPI_LOG_ERROR, format __VA_OPT__(, ) __VA_ARGS__)
+#define UVCWARNING(format, ...) \
+  UVCLOG(::wpi::WPI_LOG_WARNING, format __VA_OPT__(, ) __VA_ARGS__)
+#define UVCINFO(format, ...) \
+  UVCLOG(::wpi::WPI_LOG_INFO, format __VA_OPT__(, ) __VA_ARGS__)
+
+#ifdef NDEBUG
+#define UVCDEBUG(format, ...) \
+  do {                         \
+  } while (0)
+#define UVCDEBUG1(format, ...) \
+  do {                          \
+  } while (0)
+#define UVCDEBUG2(format, ...) \
+  do {                          \
+  } while (0)
+#define UVCDEBUG3(format, ...) \
+  do {                          \
+  } while (0)
+#define UVCDEBUG4(format, ...) \
+  do {                          \
+  } while (0)
+#else
+#define UVCDEBUG(format, ...) \
+  UVCLOG(::wpi::WPI_LOG_DEBUG, format __VA_OPT__(, ) __VA_ARGS__)
+#define UVCDEBUG1(format, ...) \
+  UVCLOG(::wpi::WPI_LOG_DEBUG1, format __VA_OPT__(, ) __VA_ARGS__)
+#define UVCDEBUG2(format, ...) \
+  UVCLOG(::wpi::WPI_LOG_DEBUG2, format __VA_OPT__(, ) __VA_ARGS__)
+#define UVCDEBUG3(format, ...) \
+  UVCLOG(::wpi::WPI_LOG_DEBUG3, format __VA_OPT__(, ) __VA_ARGS__)
+#define UVCDEBUG4(format, ...) \
+  UVCLOG(::wpi::WPI_LOG_DEBUG4, format __VA_OPT__(, ) __VA_ARGS__)
+#endif
+
+// USB descriptor for UVC processing unit
+struct ProcessingUnitDescriptor
+{
+    uint8_t bLength;
+    uint8_t bDescriptorType;        // CS_INTERFACE 0x24
+    uint8_t bDescriptorSubtype;     // VC_PROCESSING_UNIT 0x05
+    uint8_t bUnitID;
+};
+
+struct propertyInfo_t
+{
+    uint32_t    selector;   // selector ID
+    uint32_t    unit;       // unit (==0 for INPUT TERMINA:, ==1 for PROCESSING UNIT)
+    uint32_t    length;     // length (bytes)
+};
+
+/** The order of the propertyInfo structure must
+    be the same as the PROPID numbers in the
+    openpnp-capture.h header */
+const propertyInfo_t propertyInfo[] =
+{
+    {0,0,0},
+    {CT_EXPOSURE_TIME_ABSOLUTE_CONTROL   , 0, 4},
+    {CT_FOCUS_ABSOLUTE_CONTROL           , 0, 2},
+    {CT_ZOOM_ABSOLUTE_CONTROL            , 0, 2},
+    {PU_WHITE_BALANCE_TEMPERATURE_CONTROL, 1, 2},
+    {PU_GAIN_CONTROL                     , 1, 2},
+    {PU_BRIGHTNESS_CONTROL               , 1, 2},
+    {PU_CONTRAST_CONTROL                 , 1, 2},
+    {PU_SATURATION_CONTROL               , 1, 2},
+    {PU_GAMMA_CONTROL                    , 1, 2},
+    {PU_HUE_CONTROL                      , 1, 2},
+    {PU_SHARPNESS_CONTROL                , 1, 2},
+    {PU_BACKLIGHT_COMPENSATION_CONTROL   , 1, 2},
+    {PU_POWER_LINE_FREQUENCY_CONTROL     , 1, 1}
+};
+
+@implementation UvcControlImpl {
+    IOUSBDeviceInterface** _deviceInterface;
+}
+
+
++ (instancetype)createFromAVCaptureDevice:(AVCaptureDevice*)device status:(CS_Status*)status {
+    if (!device) {
+        NSLog(@"UVC: device is nil");
+        *status = CS_UVC_STATUS_ERROR;
+        return nil;
+    }
+    
+    NSError* error = nil;
+    NSRegularExpression* regex = [NSRegularExpression regularExpressionWithPattern:@"^UVC\\s+Camera\\s+VendorID\\_([0-9]+)\\s+ProductID\\_([0-9]+)$" 
+                                                                         options:0 
+                                                                           error:&error];
+    if (error) {
+        NSLog(@"UVC: failed to create regex: %@", error);
+        *status = CS_UVC_STATUS_ERROR;
+        return nil;
+    }
+    
+    NSString* modelID = [device valueForKey:@"modelID"];
+    if (!modelID) {
+        NSLog(@"UVC: modelID is nil");
+        *status = CS_UVC_STATUS_ERROR;
+        return nil;
+    }
+    
+    NSTextCheckingResult* match = [regex firstMatchInString:modelID 
+                                                   options:0 
+                                                     range:NSMakeRange(0, modelID.length)];
+    if (!match || match.numberOfRanges != 3) {
+        NSLog(@"UVC: modelID regex match failed");
+        *status = CS_UVC_STATUS_ERROR;
+        return nil;
+    }
+    
+    NSString* vendorIDStr = [modelID substringWithRange:[match rangeAtIndex:1]];
+    NSString* productIDStr = [modelID substringWithRange:[match rangeAtIndex:2]];
+    uint16_t vendorID = (uint16_t)strtoul([vendorIDStr UTF8String], NULL, 10);
+    uint16_t productID = (uint16_t)strtoul([productIDStr UTF8String], NULL, 10);
+    
+    uint32_t locationID = 0;
+    CFMutableDictionaryRef dict = IOServiceMatching(kIOUSBDeviceClassName);
+    CFDictionarySetValue(dict, CFSTR("idVendor"), (__bridge CFNumberRef)@(vendorID));
+    CFDictionarySetValue(dict, CFSTR("idProduct"), (__bridge CFNumberRef)@(productID));
+    
+    io_iterator_t iter = 0;
+    kern_return_t ioResult = IOServiceGetMatchingServices(kIOMainPortDefault, dict, &iter);
+    if (ioResult == kIOReturnSuccess) {
+        io_service_t usbDevice = IOIteratorNext(iter);
+        while (usbDevice != 0) {
+            CFTypeRef locationIDRef = IORegistryEntryCreateCFProperty(usbDevice, 
+                                                                    CFSTR("locationID"), 
+                                                                    kCFAllocatorDefault, 
+                                                                    0);
+            if (locationIDRef) {
+                locationID = [(__bridge NSNumber*)locationIDRef unsignedIntValue];
+                CFRelease(locationIDRef);
+                NSString* uniqueID = [device valueForKey:@"uniqueID"];
+                NSString* locationIDHex = [NSString stringWithFormat:@"0x%x", locationID];
+                if ([uniqueID hasPrefix:locationIDHex]) {
+                    IOObjectRelease(usbDevice);
+                    break;
+                }
+            }
+            IOObjectRelease(usbDevice);
+            usbDevice = IOIteratorNext(iter);
+        }
+        IOObjectRelease(iter);
+    }
+    
+    UvcControlImpl *instance = [[UvcControlImpl alloc] initWithVendorId:vendorID 
+                                        productId:productID 
+                                        location:locationID 
+                                           status:status];
+    if (!instance) {
+        NSLog(@"UVC: failed to create UvcControlImpl, status=%d", *status);
+    }
+    return instance;
+}
+
+- (instancetype)initWithVendorId:(uint16_t)vid 
+                      productId:(uint16_t)pid 
+                      location:(uint32_t)location
+                         status:(CS_Status*)status {
+    self = [super init];
+    if (self) {
+        // UVCINFO("Initializing with VID: 0x{:04X}, PID: 0x{:04X}, Location: 0x{:08X}", vid, pid, location);
+        _deviceInterface = [self findDevice:vid productId:pid location:location];
+        if (_deviceInterface == nullptr) {
+            // UVCWARNING("Failed to find device");
+            *status = CS_UVC_STATUS_DEVICE_DISCONNECTED;
+            return nil;
+        }
+        
+        _processingUnitID = [self getProcessingUnitID:_deviceInterface];
+        
+        _controlInterface = [self createControlInterface:_deviceInterface];
+        
+        if (_controlInterface == nullptr) {
+            // UVCWARNING("Failed to create control interface");
+            *status = CS_UVC_STATUS_DEVICE_DISCONNECTED;
+            return nil;
+        }
+    }
+    return self;
+}
+
+- (void)dealloc {
+    if (_controlInterface != nullptr) {
+        (*_controlInterface)->USBInterfaceClose(_controlInterface);
+        (*_controlInterface)->Release(_controlInterface);
+    }
+    if (_deviceInterface != nullptr) {
+        (*_deviceInterface)->Release(_deviceInterface);
+    }
+}
+
+- (IOUSBDeviceInterface**)findDevice:(uint16_t)vid 
+                          productId:(uint16_t)pid 
+                          location:(uint32_t)location {
+
+    CFMutableDictionaryRef dict = IOServiceMatching(kIOUSBDeviceClassName);
+    
+    io_iterator_t serviceIterator;
+    kern_return_t result = IOServiceGetMatchingServices((mach_port_t)NULL, dict, &serviceIterator);
+    if (result != kIOReturnSuccess) {
+        UVCERROR("findDevice: IOServiceGetMatchingServices failed: {}", result);
+        return nullptr;
+    }
+    
+    io_service_t device;
+    while((device = IOIteratorNext(serviceIterator)) != 0) {
+        IOUSBDeviceInterface **deviceInterface = nullptr;
+        IOCFPlugInInterface  **plugInInterface = nullptr;
+        SInt32 score;
+        
+        kern_return_t result = IOCreatePlugInInterfaceForService(
+            device, kIOUSBDeviceUserClientTypeID,
+            kIOCFPlugInInterfaceID, &plugInInterface, &score);
+            
+        if ((result != kIOReturnSuccess) || (plugInInterface == nullptr)) {
+            UVCERROR("findDevice: Camera control error: {}", result);
+            IOObjectRelease(device);
+            continue;
+        }
+        
+        HRESULT hr = (*plugInInterface)->QueryInterface(plugInInterface,
+            CFUUIDGetUUIDBytes(kIOUSBDeviceInterfaceID),
+            (LPVOID*)&deviceInterface);
+            
+        if (hr || (deviceInterface == nullptr)) {
+            (*plugInInterface)->Release(plugInInterface);
+            IOObjectRelease(device);
+            UVCERROR("findDevice: QueryInterface failed");
+            continue;
+        }
+        
+        uint16_t vendorID, productID;
+        uint32_t locationID;
+        result = (*deviceInterface)->GetDeviceVendor(deviceInterface, &vendorID);
+        result = (*deviceInterface)->GetDeviceProduct(deviceInterface, &productID);
+        result = (*deviceInterface)->GetLocationID(deviceInterface, &locationID);
+        
+        // if 'location' is zero, we won't match on location
+        // to achieve this, we simply set locationID to zero.
+        if (location == 0) {
+            locationID = 0;
+        }
+        
+        if ((vendorID == vid) && (productID == pid) && (locationID == location)) {
+            (*plugInInterface)->Release(plugInInterface);
+            IOObjectRelease(device);
+            IOObjectRelease(serviceIterator);
+            return deviceInterface;
+        }
+        
+        (*deviceInterface)->Release(deviceInterface);
+        (*plugInInterface)->Release(plugInInterface);
+        IOObjectRelease(device);
+    }
+    
+    IOObjectRelease(serviceIterator);
+    return nullptr;
+}
+
+- (uint32_t)getProcessingUnitID:(IOUSBDeviceInterface**)dev {
+    IOReturn kr;
+    IOUSBConfigurationDescriptorPtr configDesc;
+    
+    kr = (*dev)->GetConfigurationDescriptorPtr(dev, 0, &configDesc);
+    if (kr) {
+        return 0;
+    }
+
+    UVCDEBUG4("USB descriptor:");
+    UVCDEBUG4("  length    = 0x{:08X}", configDesc->bLength);
+    UVCDEBUG4("  type      = 0x{:08X}", configDesc->bDescriptorType);
+    UVCDEBUG4("  totalLen  = 0x{:08X}", configDesc->wTotalLength);
+    UVCDEBUG4("  interfaces = 0x{:08X}", configDesc->bNumInterfaces);
+    
+    uint32_t idx = 0;
+    uint8_t *ptr = (uint8_t*)configDesc;
+
+    // Search for VIDEO/CONTROL interface descriptor
+    // Class=14, Subclass=1, Protocol=0
+    // and find the processing unit, if available..
+        // DescriptorType 0x24, DescriptorSubType 0x5
+
+    IOUSBInterfaceDescriptor *iface = NULL;
+    ProcessingUnitDescriptor *pud   = NULL;
+    bool inVideoControlInterfaceDescriptor = false;
+    while(idx < configDesc->wTotalLength) {
+        IOUSBDescriptorHeader *hdr = (IOUSBDescriptorHeader *)&ptr[idx];
+        switch(hdr->bDescriptorType)
+            {
+                case 0x05:  // Endpoint descriptor ID
+                    break;
+                case 0x02:  // Configuration descriptor ID
+                    break;
+                case 0x04: // Interface descriptor ID
+                    iface = (IOUSBInterfaceDescriptor*)&ptr[idx];
+                    if ((iface->bInterfaceClass == 14) && 
+                        (iface->bInterfaceSubClass == 1) &&
+                        (iface->bInterfaceProtocol == 0))
+                    {
+                        inVideoControlInterfaceDescriptor = true;
+                    }
+                    else
+                    {
+                        inVideoControlInterfaceDescriptor = false;
+                    }
+                    break;
+                case 0x24: // class-specific ID
+                    pud = (ProcessingUnitDescriptor*)&ptr[idx];
+                    if (inVideoControlInterfaceDescriptor)
+                    {
+                        if (pud->bDescriptorSubtype == 0x05)
+                        {
+                            return pud->bUnitID;
+                        }
+                    }
+                    break;
+                default:
+                    break;
+            }
+        idx += hdr->bLength;
+    }
+    
+    return 0;
+}
+
+- (IOUSBInterfaceInterface190**)createControlInterface:(IOUSBDeviceInterface**)deviceInterface {
+    IOUSBInterfaceInterface190 **controlInterface;
+    
+    io_iterator_t interfaceIterator;
+    IOUSBFindInterfaceRequest interfaceRequest;
+    interfaceRequest.bInterfaceClass = UVC_CONTROL_INTERFACE_CLASS;
+    interfaceRequest.bInterfaceSubClass = UVC_CONTROL_INTERFACE_SUBCLASS;
+    interfaceRequest.bInterfaceProtocol = kIOUSBFindInterfaceDontCare;
+    interfaceRequest.bAlternateSetting = kIOUSBFindInterfaceDontCare;
+    
+    IOReturn result = (*deviceInterface)->CreateInterfaceIterator(deviceInterface,
+        &interfaceRequest, &interfaceIterator);
+        
+    if (result != kIOReturnSuccess) {
+        return nullptr;
+    }
+    
+    io_service_t usbInterface;
+    if ((usbInterface = IOIteratorNext(interfaceIterator)) != 0) {
+        IOCFPlugInInterface **plugInInterface = nullptr;
+        SInt32 score;
+        
+        kern_return_t kr = IOCreatePlugInInterfaceForService(usbInterface,
+            kIOUSBInterfaceUserClientTypeID,
+            kIOCFPlugInInterfaceID,
+            &plugInInterface,
+            &score);
+            
+        kr = IOObjectRelease(usbInterface);
+        if ((kr != kIOReturnSuccess) || !plugInInterface) {
+            UVCERROR("createControlInterface: cannot create plug-in {:08X}",
+                kr);
+            return nullptr;
+        }
+        
+        HRESULT hr = (*plugInInterface)->QueryInterface(plugInInterface,
+            CFUUIDGetUUIDBytes(kIOUSBInterfaceInterfaceID),
+            (LPVOID*) &controlInterface);
+            
+        (*plugInInterface)->Release(plugInInterface);
+        
+        if (hr || !controlInterface) {
+            UVCERROR("createControlInterface: cannot create device interface {:08X}",
+                result);
+            return nullptr;
+        }
+        
+        UVCDEBUG3("createControlInterface: created control interface");
+        return controlInterface;
+    }
+    return nullptr;
+}
+
+- (bool)sendControlRequest:(IOUSBDevRequest)req {
+    if (_controlInterface == nullptr) {
+        UVCERROR("control interface is NULL");
+        return false;
+    }
+
+    kern_return_t kr;
+    if (@available(macOS 12.0, *)) {
+        // macOS 12 doesn't like if we're trying to open USB interface here...
+    } else {
+        kr = (*_controlInterface)->USBInterfaceOpen(_controlInterface);
+        if (kr != kIOReturnSuccess) {
+            UVCERROR("USBInterfaceOpen failed with error: 0x{:08X}", kr);
+            return false;
+        }
+    }
+
+    kr = (*_controlInterface)->ControlRequest(_controlInterface, 0, &req);
+    if (kr != kIOReturnSuccess) {
+        // IOKIT error code
+        #define err_get_system(err) (((err)>>26)&0x3f) 
+        #define err_get_sub(err) (((err)>>14)&0xfff) 
+        #define err_get_code(err) ((err)&0x3fff)
+        
+        uint32_t code = err_get_code(kr);
+        uint32_t sys  = err_get_system(kr);
+        uint32_t sub  = err_get_sub(kr);
+
+        switch(kr)
+        {
+            case kIOUSBUnknownPipeErr:
+                UVCERROR("Pipe ref not recognised");
+                break;
+            case kIOUSBTooManyPipesErr:
+                UVCERROR("Too many pipes");
+                break;
+            case kIOUSBEndpointNotFound:
+                UVCERROR("Endpoint not found");
+                break;
+            case kIOUSBConfigNotFound:
+                UVCERROR("USB configuration not found");
+                break;
+            case kIOUSBPipeStalled:
+                //Note: we don't report this as an error as this happens when
+                //      an unsupported or locked property is set.            
+                UVCDEBUG("Pipe has stalled, error needs to be cleared");
+                break;
+            case kIOUSBInterfaceNotFound:
+                UVCERROR("USB control interface not found");
+                break;
+            default:
+                UVCERROR("ControlRequest failed (KR=sys:sub:code) = {:02Xh}:{:03Xh}:{:04Xh}", 
+                    sys, sub, code);
+                break; 
+        }
+
+        if (@available(macOS 12.0, *)) {
+            // macOS 12 doesn't like if we're trying to close USB interface here...
+        } else {
+            kr = (*_controlInterface)->USBInterfaceClose(_controlInterface);
+            if (kr != kIOReturnSuccess) {
+                UVCERROR("USBInterfaceClose failed");
+            }
+        }
+        return false;
+    }
+
+    if (@available(macOS 12.0, *)) {
+        // macOS 12 doesn't like if we're trying to close USB interface here either...
+    } else {
+        kr = (*_controlInterface)->USBInterfaceClose(_controlInterface);
+
+        if (kr != kIOReturnSuccess) {
+            UVCERROR("USBInterfaceClose failed");
+        }
+    }
+
+    return true;
+}
+
+- (bool)setData:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t)data {
+    IOUSBDevRequest req;
+    req.bmRequestType = USBmakebmRequestType((UInt8)kUSBOut, (UInt8)kUSBClass, (UInt8)kUSBInterface);
+    req.bRequest = UVC_SET_CUR;
+    req.wValue = (selector << 8);
+    req.wIndex = (unit << 8);
+    req.wLength = length;
+    req.wLenDone = 0;
+    req.pData = &data;
+    return [self sendControlRequest:req];
+}
+
+- (bool)getData:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t*)data {
+    IOUSBDevRequest req;
+    req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface);
+    req.bRequest = UVC_GET_CUR;
+    req.wValue = (selector << 8);
+    req.wIndex = (unit << 8);
+    req.wLength = length;
+    req.wLenDone = 0;
+    req.pData = data;
+    return [self sendControlRequest:req];
+}
+
+- (bool)getMaxData:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t*)data {
+    IOUSBDevRequest req;
+    *data = 0;
+    req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface);
+    req.bRequest = UVC_GET_MAX;
+    req.wValue = (selector << 8);
+    req.wIndex = (unit << 8);
+    req.wLength = length;
+    req.wLenDone = 0;
+    req.pData = data;
+    return [self sendControlRequest:req];
+}
+
+- (bool)getMinData:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t*)data {
+    IOUSBDevRequest req;
+    *data = 0;
+    req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface);
+    req.bRequest = UVC_GET_MIN;
+    req.wValue = (selector << 8);
+    req.wIndex = (unit << 8);
+    req.wLength = length;
+    req.wLenDone = 0;
+    req.pData = data;
+    return [self sendControlRequest:req];
+}
+
+- (bool)getDefault:(uint32_t)selector unit:(uint32_t)unit length:(uint32_t)length data:(int32_t*)data {
+    IOUSBDevRequest req;
+    *data = 0;
+    req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface);
+    req.bRequest = UVC_GET_DEF;
+    req.wValue = (selector << 8);
+    req.wIndex = (unit << 8);
+    req.wLength = length;
+    req.wLenDone = 0;
+    req.pData = data;
+    return [self sendControlRequest:req];
+}
+
+- (bool)getInfo:(uint32_t)selector unit:(uint32_t)unit data:(uint32_t*)data {
+    IOUSBDevRequest req;
+    *data = 0;
+    req.bmRequestType = USBmakebmRequestType((UInt8)kUSBIn, (UInt8)kUSBClass, (UInt8)kUSBInterface);
+    req.bRequest = UVC_GET_INFO;
+    req.wValue = (selector << 8);
+    req.wIndex = (unit << 8);
+    req.wLength = 1;
+    req.wLenDone = 0;
+    req.pData = data;
+    return [self sendControlRequest:req];
+}
+
+- (bool)setProperty:(uint32_t)propID withValue:(int32_t)value status:(CS_Status*)status {
+    if (_controlInterface == nullptr) {
+        UVCERROR("control interface is NULL");
+        *status = CS_UVC_STATUS_DEVICE_DISCONNECTED;
+        return false;
+    }
+
+    bool ok = false;
+    if (propID < CAPPROPID_LAST) {
+        uint32_t unit = (propertyInfo[propID].unit == 0) ? UVC_INPUT_TERMINAL_ID : _processingUnitID;
+        ok = [self setData:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:value];
+        if (!ok) {
+            UVCWARNING("Failed to set property {}", propID);
+        }
+    } else {
+        UVCWARNING("Invalid property ID: {}", propID);
+    }
+    return ok;
+}
+
+- (bool)getProperty:(uint32_t)propID withValue:(int32_t*)value status:(CS_Status*)status {
+    if (_controlInterface == nullptr) {
+        UVCERROR("control interface is NULL");
+        *status = CS_UVC_STATUS_DEVICE_DISCONNECTED;
+        return false;
+    }
+
+    bool ok = false;
+    if (propID < CAPPROPID_LAST) {
+        uint32_t unit = (propertyInfo[propID].unit == 0) ? UVC_INPUT_TERMINAL_ID : _processingUnitID;
+        ok = [self getData:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:value];
+
+        switch(propertyInfo[propID].length) {
+            case 2:
+                *value = static_cast<int16_t>(*value);
+                break;
+            case 1:
+                *value = static_cast<int8_t>(*value);
+                break;
+            default:
+                break;
+        }
+        if (!ok) {
+            UVCWARNING("Failed to get property {}", propID);
+        }
+    } else {
+        UVCWARNING("Invalid property ID: {}", propID);
+    }
+    return ok;
+}
+
+- (bool)setAutoProperty:(uint32_t)propID enabled:(bool)enabled status:(CS_Status*)status {
+    if (_controlInterface == nullptr) {
+        UVCERROR("control interface is NULL");
+        *status = CS_UVC_STATUS_DEVICE_DISCONNECTED;
+        return false;
+    }
+
+    int32_t value = enabled ? 1 : 0;
+    switch(propID) {
+        case CAPPROPID_EXPOSURE:
+            return [self setData:CT_AE_MODE_CONTROL unit:UVC_INPUT_TERMINAL_ID length:1 data:enabled ? 0x8 : 0x1];
+        case CAPPROPID_WHITEBALANCE:
+            return [self setData:PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL unit:_processingUnitID length:1 data:value];
+        case CAPPROPID_FOCUS:
+            return [self setData:CT_FOCUS_AUTO_CONTROL unit:UVC_INPUT_TERMINAL_ID length:1 data:value];
+        default:
+            return false;
+    }
+}
+
+- (bool)getAutoProperty:(uint32_t)propID enabled:(bool*)enabled status:(CS_Status*)status {
+    if (_controlInterface == nullptr) {
+        UVCERROR("control interface is NULL");
+        *status = CS_UVC_STATUS_DEVICE_DISCONNECTED;
+        return false;
+    }
+
+    int32_t value;
+    
+    switch(propID) {
+        case CAPPROPID_EXPOSURE:
+            if ([self getData:CT_AE_MODE_CONTROL unit:UVC_INPUT_TERMINAL_ID length:1 data:&value]) {
+                // value = 1 -> manual mode
+                //         2 -> auto mode (I haven't seen this in the wild)
+                //         4 -> shutter priority mode (haven't seen this)
+                //         8 -> aperature prioritry mode (seen this used)
+                value &= 0xFF;
+                *enabled = (value==1) ? false : true;
+                return true;
+            }
+            return false;
+        case CAPPROPID_WHITEBALANCE:
+            if ([self getData:PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL unit:_processingUnitID length:1 data:&value]) {
+                value &= 0xFF;
+                *enabled = (value==1) ? true : false;
+                UVCDEBUG3("White balance auto mode: {}", *enabled ? "enabled" : "disabled");
+                return true;
+            }
+            return false;
+        case CAPPROPID_FOCUS:
+            if ([self getData:CT_FOCUS_AUTO_CONTROL unit:UVC_INPUT_TERMINAL_ID length:1 data:&value]) {
+                value &= 0xFF;
+                *enabled = (value==1) ? true : false;
+                UVCDEBUG3("Focus auto mode: {}", *enabled ? "enabled" : "disabled");
+                return true;
+            }
+            return false;
+        default:
+            UVCWARNING("Unsupported auto property ID: {}", propID);
+            return false;
+    }
+}
+
+- (bool)getPropertyLimits:(uint32_t)propID min:(int32_t*)min max:(int32_t*)max defValue:(int32_t*)defValue status:(CS_Status*)status {
+    if (_controlInterface == nullptr) {
+        *status = CS_UVC_STATUS_DEVICE_DISCONNECTED;
+        return false;
+    }
+
+    bool ok = true;
+    if (propID < CAPPROPID_LAST) {
+        uint32_t unit = (propertyInfo[propID].unit == 0) ? UVC_INPUT_TERMINAL_ID : _processingUnitID;
+        
+        if (![self getMinData:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:min]) {
+            ok = false;
+        }
+
+        if (![self getMaxData:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:max]) {
+            ok = false;
+        }
+
+        if (![self getDefault:propertyInfo[propID].selector unit:unit length:propertyInfo[propID].length data:defValue]) {
+            ok = false;
+        }
+
+        switch(propertyInfo[propID].length) {
+            case 2:
+                *min = static_cast<int16_t>(*min);
+                *max = static_cast<int16_t>(*max);
+                *defValue = static_cast<int16_t>(*defValue);
+                break;
+            case 1:
+                *min = static_cast<int8_t>(*min);
+                *max = static_cast<int8_t>(*max);
+                *defValue = static_cast<int8_t>(*defValue);
+                break;
+            default:
+                break;
+        }
+    } else {
+        UVCWARNING("getPropertyLimits: property ID out of bounds");
+        ok = false;
+    }
+    return ok;
+}
+
+- (void)reportCapabilities:(uint32_t)selector unit:(uint32_t)unit {
+    uint32_t info;
+    [self getInfo:selector unit:unit data:&info];
+    if (info & 0x01) {
+        UVCDEBUG4("GET ");
+    }
+    if (info & 0x02) {
+        UVCDEBUG4("SET ");
+    }
+    if (info & 0x04) {
+        UVCDEBUG4("DISABLED ");
+    }
+    if (info & 0x08) {
+        UVCDEBUG4("AUTO-UPD ");
+    }
+    if (info & 0x10) {
+        UVCDEBUG4("ASYNC ");
+    }
+    if (info & 0x20) {
+        UVCDEBUG4("DISCOMMIT");
+    }
+    UVCDEBUG4("");
+}
+
+@end 

datalog/src/main/java/edu/wpi/first/datalog/DataLog.java

@@ -451,10 +451,10 @@ public class DataLog implements AutoCloseable {
     if (!seen.add(typeString)) {
       throw new UnsupportedOperationException(typeString + ": circular reference with " + seen);
     }
-    addSchema(typeString, "structschema", struct.getSchema(), timestamp);
     for (Struct<?> inner : struct.getNested()) {
       addSchemaImpl(inner, timestamp, seen);
     }
+    addSchema(typeString, "structschema", struct.getSchema(), timestamp);
     seen.remove(typeString);
   }
 

datalogtool/src/main/native/cpp/App.cpp

@@ -10,11 +10,9 @@
 #include <glass/Context.h>
 #include <glass/MainMenuBar.h>
 #include <glass/Storage.h>
-#include <libssh/libssh.h>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 #include <imgui_internal.h>
+#include <libssh/libssh.h>
 #include <wpigui.h>
 #include <wpigui_openurl.h>
 

datalogtool/src/main/native/cpp/App.h

@@ -4,7 +4,6 @@
 
 #pragma once
 
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 
 void SetNextWindowPos(const ImVec2& pos, ImGuiCond cond = 0,

glass/.styleguide

@@ -26,8 +26,10 @@ includeOtherLibs {
   ^frc/
   ^google/
   ^imgui
+  portable-file-dialogs.h
   ^networktables/
   ^ntcore
+  ^units
   ^wpi/
   ^wpigui
   ^wpimath/

glass/src/lib/native/cpp/hardware/Gyro.cpp

@@ -7,8 +7,6 @@
 #include <cmath>
 #include <numbers>
 
-#define IMGUI_DEFINE_MATH_OPERATORS
-
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <wpi/StringExtras.h>

glass/src/lib/native/cpp/other/Drive.cpp

@@ -6,12 +6,10 @@
 
 #include <array>
 #include <cmath>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
+#include <numbers>
 
 #include <imgui.h>
 #include <imgui_internal.h>
-#include <numbers>
 
 #include "glass/DataSource.h"
 

glass/src/lib/native/cpp/other/Field2D.cpp

@@ -17,8 +17,6 @@
 #include <frc/geometry/Pose2d.h>
 #include <frc/geometry/Rotation2d.h>
 #include <frc/geometry/Translation2d.h>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <imgui_stdlib.h>

glass/src/lib/native/cpp/other/Mechanism2D.cpp

@@ -17,8 +17,6 @@
 #include <frc/geometry/Rotation2d.h>
 #include <frc/geometry/Transform2d.h>
 #include <frc/geometry/Translation2d.h>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <imgui_stdlib.h>

glass/src/lib/native/cpp/other/Plot.cpp

@@ -22,7 +22,6 @@
 #pragma GCC diagnostic ignored "-Wformat-nonliteral"
 #endif
 
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 #include <imgui_stdlib.h>
 #include <implot.h>

glass/src/lib/native/cpp/support/ExtraGuiWidgets.cpp

@@ -4,14 +4,12 @@
 
 #include "glass/support/ExtraGuiWidgets.h"
 
-#define IMGUI_DEFINE_MATH_OPERATORS
-#include <imgui.h>
-#include <imgui_internal.h>
-
 #include <cstdio>
 #include <cstdlib>
 #include <cstring>
 
+#include <imgui.h>
+#include <imgui_internal.h>
 #include <wpi/DenseMap.h>
 
 #include "glass/DataSource.h"

glass/src/lib/native/include/glass/MainMenuBar.h

@@ -4,12 +4,12 @@
 
 #pragma once
 
-#include <portable-file-dialogs.h>
-
 #include <functional>
 #include <memory>
 #include <vector>
 
+#include <portable-file-dialogs.h>
+
 namespace glass {
 
 /**

glass/src/lib/native/include/glass/Window.h

@@ -9,7 +9,6 @@
 #include <string_view>
 #include <utility>
 
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 
 #include "glass/View.h"

glass/src/lib/native/include/glass/other/Field2D.h

@@ -10,8 +10,6 @@
 #include <frc/geometry/Pose2d.h>
 #include <frc/geometry/Rotation2d.h>
 #include <frc/geometry/Translation2d.h>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 #include <wpi/function_ref.h>
 

glass/src/lib/native/include/glass/other/Mechanism2D.h

@@ -6,8 +6,6 @@
 
 #include <frc/geometry/Rotation2d.h>
 #include <frc/geometry/Translation2d.h>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 #include <wpi/function_ref.h>
 

glass/src/lib/native/include/glass/support/ExtraGuiWidgets.h

@@ -6,7 +6,6 @@
 
 #include <stdint.h>
 
-#define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 
 namespace glass {

ntcore/build.gradle

@@ -33,15 +33,9 @@ model {
             if (!it.buildable || !(it instanceof NativeBinarySpec)) {
                 return
             }
-            if (it.component.name == "${nativeName}JNI") {
-                lib project: ':wpinet', library: 'wpinet', linkage: 'static'
-                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
-                lib project: ':datalog', library: 'datalog', linkage: 'static'
-            } else {
-                lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
-                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-                lib project: ':datalog', library: 'datalog', linkage: 'shared'
-            }
+            lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
+            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            lib project: ':datalog', library: 'datalog', linkage: 'shared'
         }
     }
 }

shared/config.gradle

@@ -31,6 +31,13 @@ nativeUtils.enableSourceLink()
 
 nativeUtils.wpi.addMacMinimumVersionArg()
 
+nativeUtils.platformConfigs.each {
+    if (it.name.contains('osx')) {
+        it.linker.getArgs().add('-framework')
+        it.linker.getArgs().add('IOKit')
+    }
+}
+
 // Compress debug info on Linux
 nativeUtils.platformConfigs.each {
   if (it.name.contains('linux')) {

shared/examplecheck.gradle

@@ -74,7 +74,7 @@ def tagList = [
         /* --- Hardware --- */
         "Analog", "Gyro", "Pneumatics", "I2C", "Duty Cycle", "PDP",
         "AddressableLEDs", "HAL", "Encoder", "Smart Motor Controller", "Digital Input",
-        "Digital Output",
+        "Digital Output", "Accelerometer",
 
         /* --- HID --- */
         "XboxController", "PS4Controller", "PS5Controller", "Joystick",

shared/jni/publish.gradle

@@ -7,7 +7,6 @@ def baseArtifactId = nativeName
 def artifactGroupId = "edu.wpi.first.${nativeName}"
 def zipBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-cpp_CLS"
 ext.zipBaseName = zipBaseName
-def jniBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jni_CLS"
 def jniCvStaticBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jnicvstatic_CLS"
 
 def licenseFile = file("$rootDir/license.md")
@@ -77,28 +76,6 @@ model {
             "${nativeName}JNIShared"
         ], zipBaseName, Zip, project, includeStandardZipFormat)
 
-        def jniTaskList = createComponentZipTasks($.components, ["${nativeName}JNI"], jniBaseName, Jar, project, { task, value ->
-            value.each { binary ->
-                if (binary.buildable) {
-                    if (binary instanceof SharedLibraryBinarySpec) {
-                        task.dependsOn binary.tasks.link
-                        def hashFile = new File(binary.sharedLibraryFile.parentFile.absolutePath, "${binary.component.baseName}.hash")
-                        task.outputs.file(hashFile)
-                        task.inputs.file(binary.sharedLibraryFile)
-                        task.from(hashFile) {
-                            into nativeUtils.getPlatformPath(binary)
-                        }
-                        task.doFirst {
-                            hashFile.text = MessageDigest.getInstance("MD5").digest(binary.sharedLibraryFile.bytes).encodeHex().toString()
-                        }
-                        task.from(binary.sharedLibraryFile) {
-                            into nativeUtils.getPlatformPath(binary)
-                        }
-                    }
-                }
-            }
-        })
-
         publications {
             cpp(MavenPublication) {
                 taskList.each {
@@ -111,15 +88,6 @@ model {
                 groupId artifactGroupId
                 version wpilibVersioning.version.get()
             }
-            jni(MavenPublication) {
-                jniTaskList.each {
-                    artifact it
-                }
-
-                artifactId = "${baseArtifactId}-jni"
-                groupId artifactGroupId
-                version wpilibVersioning.version.get()
-            }
         }
 
         if (project.hasProperty('cvStaticBuild') && project.getProperty('cvStaticBuild') == true) {

shared/jni/setupBuild.gradle

@@ -145,53 +145,6 @@ model {
                 }
             }
         }
-        "${nativeName}JNI"(JniNativeLibrarySpec) {
-            if (project.hasProperty('setBaseName')) {
-                baseName = setBaseName + 'jni'
-            } else {
-                baseName = nativeName + 'jni'
-            }
-
-            if (project.hasProperty('skipJniSymbols')) {
-                checkSkipSymbols = skipJniSymbols
-            }
-
-            enableCheckTask !project.hasProperty('skipJniCheck')
-            javaCompileTasks << compileJava
-            jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.systemcore)
-            jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm32)
-            jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm64)
-            sources {
-                cpp {
-                    source {
-                        srcDirs 'src/main/native/cpp'
-                        if (project.hasProperty('generatedSources')) {
-                            srcDir generatedSources
-                        }
-                        include '**/jni/**/*.cpp'
-                    }
-                    exportedHeaders {
-                        srcDir 'src/main/native/include'
-                        if (project.hasProperty('generatedHeaders')) {
-                            srcDir generatedHeaders
-                        }
-                        include '**/*.h'
-                    }
-                }
-            }
-            binaries.all {
-                if (it instanceof StaticLibraryBinarySpec) {
-                    it.buildable = false
-                    return
-                }
-                if (!project.hasProperty('noWpiutil')) {
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
-                }
-                if (project.hasProperty('jniSplitSetup')) {
-                    jniSplitSetup(it)
-                }
-            }
-        }
         // By default, a development executable will be generated. This is to help the case of
         // testing specific functionality of the library.
         "${nativeName}Dev"(NativeExecutableSpec) {

thirdparty/imgui_suite/imgui/include/imconfig.h

@@ -98,7 +98,7 @@
         operator MyVec4() const { return MyVec4(x,y,z,w); }
 */
 //---- ...Or use Dear ImGui's own very basic math operators.
-//#define IMGUI_DEFINE_MATH_OPERATORS
+#define IMGUI_DEFINE_MATH_OPERATORS
 
 //---- Use 32-bit vertex indices (default is 16-bit) is one way to allow large meshes with more than 64K vertices.
 // Your renderer backend will need to support it (most example renderer backends support both 16/32-bit indices).

upstream_utils/apriltag.py

@@ -1,9 +1,14 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, comment_out_invalid_includes, walk_cwd_and_copy_if
+from upstream_utils import (
+    Lib,
+    comment_out_invalid_includes,
+    has_prefix,
+    walk_cwd_and_copy_if,
+)
 
 
 def remove_tag(f: str):
@@ -18,47 +23,45 @@ def remove_tag(f: str):
     return False
 
 
-def copy_upstream_src(wpilib_root):
-    apriltag = os.path.join(wpilib_root, "apriltag")
+def copy_upstream_src(wpilib_root: Path):
+    apriltag = wpilib_root / "apriltag"
 
     # Delete old install
     shutil.rmtree(
-        os.path.join(apriltag, "src/main/native/thirdparty/apriltag"),
+        apriltag / "src/main/native/thirdparty/apriltag",
         ignore_errors=True,
     )
     shutil.rmtree(
-        os.path.join(apriltag, "src/main/include/thirdparty/apriltag"),
+        apriltag / "src/main/include/thirdparty/apriltag",
         ignore_errors=True,
     )
 
     # Copy apriltag source files into allwpilib
     src_files = walk_cwd_and_copy_if(
         lambda dp, f: (f.endswith(".c") or f.endswith(".cpp"))
-        and not dp.startswith(os.path.join(".", "example"))
-        and not dp.startswith(os.path.join(".", "test"))
-        and not f.endswith("getopt.c")
+        and not has_prefix(dp, Path("example"))
+        and not has_prefix(dp, Path("test"))
+        and not f == "getopt.c"
         and not "py" in f
         and not remove_tag(f),
-        os.path.join(apriltag, "src/main/native/thirdparty/apriltag/src"),
+        apriltag / "src/main/native/thirdparty/apriltag/src",
     )
 
     # Copy apriltag header files into allwpilib
     walk_cwd_and_copy_if(
         lambda dp, f: f.endswith(".h")
-        and not f.endswith("getopt.h")
-        and not f.endswith("postscript_utils.h")
+        and not f == "getopt.h"
+        and not f == "postscript_utils.h"
         and not remove_tag(f),
-        os.path.join(apriltag, "src/main/native/thirdparty/apriltag/include"),
+        apriltag / "src/main/native/thirdparty/apriltag/include",
     )
 
     for f in src_files:
         comment_out_invalid_includes(
             f,
             [
-                os.path.join(apriltag, "src/main/native/thirdparty/apriltag/include"),
-                os.path.join(
-                    apriltag, "src/main/native/thirdparty/apriltag/include/common"
-                ),
+                apriltag / "src/main/native/thirdparty/apriltag/include",
+                apriltag / "src/main/native/thirdparty/apriltag/include/common",
             ],
         )
 

upstream_utils/argparse_lib.py

@@ -1,18 +1,17 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
 from upstream_utils import Lib
 
 
-def copy_upstream_src(wpilib_root):
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    wpiutil = wpilib_root / "wpiutil"
 
     # Copy header into allwpilib
-    dest_filename = os.path.join(
-        wpiutil,
-        f"src/main/native/thirdparty/argparse/include/wpi/argparse.h",
+    dest_filename = (
+        wpiutil / f"src/main/native/thirdparty/argparse/include/wpi/argparse.h"
     )
     shutil.copyfile("include/argparse/argparse.hpp", dest_filename)
     # Rename namespace from argparse to wpi

upstream_utils/debugging.py

@@ -1,26 +1,25 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, walk_cwd_and_copy_if
+from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    wpiutil = wpilib_root / "wpiutil"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/debugging/src",
         "src/main/native/thirdparty/debugging/include",
     ]:
-        shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True)
+        shutil.rmtree(wpiutil / d, ignore_errors=True)
 
     # Copy debugging files into allwpilib
     filenames = walk_cwd_and_copy_if(
-        lambda dp, f: dp.startswith(os.path.join(".", "src"))
-        or dp.startswith(os.path.join(".", "include")),
-        os.path.join(wpiutil, "src/main/native/thirdparty/debugging"),
+        lambda dp, f: has_prefix(dp, Path("src")) or has_prefix(dp, Path("include")),
+        wpiutil / "src/main/native/thirdparty/debugging",
     )
 
     for filename in filenames:

upstream_utils/eigen.py

@@ -1,13 +1,17 @@
 #!/usr/bin/env python3
 
-import os
-import re
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, comment_out_invalid_includes, walk_cwd_and_copy_if
+from upstream_utils import (
+    Lib,
+    comment_out_invalid_includes,
+    has_prefix,
+    walk_cwd_and_copy_if,
+)
 
 
-def eigen_inclusions(dp, f):
+def eigen_inclusions(dp: Path, f: str):
     """Returns true if the given file in the "Eigen" include directory of the
     Eigen git repo should be copied into allwpilib
 
@@ -15,11 +19,9 @@ def eigen_inclusions(dp, f):
     dp -- directory path
     f -- filename
     """
-    if not dp.startswith(os.path.join(".", "Eigen")):
+    if not has_prefix(dp, Path("Eigen")):
         return False
 
-    abspath = os.path.join(dp, f)
-
     # Exclude NonMPL2.h since all non-MPL2 code will be excluded anyway
     if f == "NonMPL2.h":
         return False
@@ -36,13 +38,13 @@ def eigen_inclusions(dp, f):
     if "MKL" in f:
         return False
 
-    # Include architectures we care about
-    if "Core/arch/" in abspath:
+    # Include architectures we care about by filtering for Core/arch
+    if "Core" in dp.parts and "arch" in dp.parts:
         return (
-            "arch/AVX/" in abspath
-            or "arch/Default" in abspath
-            or "arch/NEON" in abspath
-            or "arch/SSE" in abspath
+            "AVX" in dp.parts
+            or "Default" in dp.parts
+            or "NEON" in dp.parts
+            or "SSE" in dp.parts
         )
 
     # Include the following modules
@@ -65,10 +67,13 @@ def eigen_inclusions(dp, f):
         "misc",
         "plugins",
     ]
-    return bool(re.search(r"|".join("/" + m for m in modules), abspath))
+    for m in modules:
+        if m in dp.parts or f == m:
+            return True
+    return False
 
 
-def unsupported_inclusions(dp, f):
+def unsupported_inclusions(dp: Path, f: str):
     """Returns true if the given file in the "unsupported" include directory of
     the Eigen git repo should be copied into allwpilib
 
@@ -76,58 +81,58 @@ def unsupported_inclusions(dp, f):
     dp -- directory path
     f -- filename
     """
-    if not dp.startswith(os.path.join(".", "unsupported")):
+    if not has_prefix(dp, Path("unsupported")):
         return False
 
-    abspath = os.path.join(dp, f)
+    abspath = dp / f
 
     # Exclude build system and READMEs
     if f == "CMakeLists.txt" or "README" in f:
         return False
 
     # Include the MatrixFunctions module
-    return "MatrixFunctions" in abspath
+    return "MatrixFunctions" in abspath.parts
 
 
-def copy_upstream_src(wpilib_root):
-    wpimath = os.path.join(wpilib_root, "wpimath")
+def copy_upstream_src(wpilib_root: Path):
+    wpimath = wpilib_root / "wpimath"
 
     # Delete old install
     for d in ["src/main/native/thirdparty/eigen/include"]:
-        shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True)
+        shutil.rmtree(wpimath / d, ignore_errors=True)
 
     # Copy Eigen headers into allwpilib
     eigen_files = walk_cwd_and_copy_if(
         eigen_inclusions,
-        os.path.join(wpimath, "src/main/native/thirdparty/eigen/include"),
+        wpimath / "src/main/native/thirdparty/eigen/include",
     )
 
     # Copy unsupported headers into allwpilib
     unsupported_files = walk_cwd_and_copy_if(
         unsupported_inclusions,
-        os.path.join(wpimath, "src/main/native/thirdparty/eigen/include"),
+        wpimath / "src/main/native/thirdparty/eigen/include",
     )
 
     for f in eigen_files:
         comment_out_invalid_includes(
-            f, [os.path.join(wpimath, "src/main/native/thirdparty/eigen/include")]
+            f, [wpimath / "src/main/native/thirdparty/eigen/include"]
         )
     for f in unsupported_files:
         comment_out_invalid_includes(
-            f, [os.path.join(wpimath, "src/main/native/thirdparty/eigen/include")]
+            f, [wpimath / "src/main/native/thirdparty/eigen/include"]
         )
 
     shutil.copyfile(
         ".clang-format",
-        os.path.join(wpimath, "src/main/native/thirdparty/eigen/include/.clang-format"),
+        wpimath / "src/main/native/thirdparty/eigen/include/.clang-format",
     )
 
 
 def main():
     name = "eigen"
     url = "https://gitlab.com/libeigen/eigen.git"
-    # master on 2024-11-14
-    tag = "0fb2ed140d4fc0108553ecfb25f2d7fc1a9319a1"
+    # master on 2025-05-18
+    tag = "d81aa18f4dc56264b2cd7e2f230807d776a2d385"
 
     eigen = Lib(name, url, tag, copy_upstream_src)
     eigen.main()

upstream_utils/eigen_patches/0001-Disable-warnings.patch

@@ -1,17 +1,17 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 18 May 2022 09:14:24 -0700
-Subject: [PATCH 1/3] Disable warnings
+Subject: [PATCH 1/2] Disable warnings
 
 ---
- Eigen/src/Core/util/DisableStupidWarnings.h | 6 ++++++
- 1 file changed, 6 insertions(+)
+ Eigen/src/Core/util/DisableStupidWarnings.h | 9 +++++++++
+ 1 file changed, 9 insertions(+)
 
 diff --git a/Eigen/src/Core/util/DisableStupidWarnings.h b/Eigen/src/Core/util/DisableStupidWarnings.h
-index ab0c542d0e24c6ecb77abfc535c8232774cba6d5..7ecd7bf8cc927d07a28c9da4ebbe1ea4d4d2b97b 100644
+index ab0c542d0e24c6ecb77abfc535c8232774cba6d5..031d75a4c8b715dafe7158fdc5c42cd4fb069235 100644
 --- a/Eigen/src/Core/util/DisableStupidWarnings.h
 +++ b/Eigen/src/Core/util/DisableStupidWarnings.h
-@@ -81,6 +81,12 @@
+@@ -81,6 +81,15 @@
  // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
  #pragma GCC diagnostic ignored "-Wattributes"
  #endif
@@ -20,6 +20,9 @@ index ab0c542d0e24c6ecb77abfc535c8232774cba6d5..7ecd7bf8cc927d07a28c9da4ebbe1ea4
 +#endif
 +#if __GNUC__ >= 12
 +#pragma GCC diagnostic ignored "-Warray-bounds"
++#endif
++#if __GNUC__ >= 13
++#pragma GCC diagnostic ignored "-Wstringop-overflow"
 +#endif
  #endif
  

upstream_utils/eigen_patches/0002-Intellisense-fix.patch

@@ -1,17 +1,17 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <johnson.peter@gmail.com>
 Date: Fri, 20 Jan 2023 23:41:56 -0800
-Subject: [PATCH 2/3] Intellisense fix
+Subject: [PATCH 2/2] Intellisense fix
 
 ---
  Eigen/src/Core/util/ConfigureVectorization.h | 7 +++++++
  1 file changed, 7 insertions(+)
 
 diff --git a/Eigen/src/Core/util/ConfigureVectorization.h b/Eigen/src/Core/util/ConfigureVectorization.h
-index 47ddd4f8ae33405f2a6600dc33bd3d07a668e63f..d0fd181ecfdeb9a80e065b1d81e8860d6a6ad57c 100644
+index 49f307c734e937f013e659e931286a17ef6756f9..a9430716a320327aed81ea0cdffabc051aeb0ce2 100644
 --- a/Eigen/src/Core/util/ConfigureVectorization.h
 +++ b/Eigen/src/Core/util/ConfigureVectorization.h
-@@ -174,6 +174,13 @@
+@@ -178,6 +178,13 @@
  
  //----------------------------------------------------------------------
  

upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch

@@ -1,305 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Tyler Veness <calcmogul@gmail.com>
-Date: Sun, 12 Jan 2025 21:04:07 -0800
-Subject: [PATCH 3/3] Make assignment constexpr
-
----
- Eigen/src/Core/AssignEvaluator.h             | 165 +++++++++++--------
- Eigen/src/Core/EigenBase.h                   |   2 +-
- Eigen/src/Core/functors/AssignmentFunctors.h |   2 +-
- 3 files changed, 102 insertions(+), 67 deletions(-)
-
-diff --git a/Eigen/src/Core/AssignEvaluator.h b/Eigen/src/Core/AssignEvaluator.h
-index f7f0b238b8ca70bbc9100262479cc1dbebab9979..9c2436afa7fe98692a036e6ef255ed104a5bf388 100644
---- a/Eigen/src/Core/AssignEvaluator.h
-+++ b/Eigen/src/Core/AssignEvaluator.h
-@@ -263,7 +263,7 @@ struct copy_using_evaluator_innervec_CompleteUnrolling {
-     DstAlignment = Kernel::AssignmentTraits::DstAlignment
-   };
- 
--  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
-+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
-     kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, inner);
-     enum { NextIndex = Index + unpacket_traits<PacketType>::size };
-     copy_using_evaluator_innervec_CompleteUnrolling<Kernel, NextIndex, Stop>::run(kernel);
-@@ -431,17 +431,25 @@ struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, NoUnrolling> {
- template <typename Kernel>
- struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, CompleteUnrolling> {
-   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
--    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
--    typedef typename Kernel::PacketType PacketType;
--
--    enum {
--      size = DstXprType::SizeAtCompileTime,
--      packetSize = unpacket_traits<PacketType>::size,
--      alignedSize = (int(size) / packetSize) * packetSize
--    };
--
--    copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
--    copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
-+    if (internal::is_constant_evaluated()) {
-+      for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
-+        for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
-+          kernel.assignCoeffByOuterInner(outer, inner);
-+        }
-+      }
-+    } else {
-+      typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
-+      typedef typename Kernel::PacketType PacketType;
-+
-+      enum {
-+        size = DstXprType::SizeAtCompileTime,
-+        packetSize = unpacket_traits<PacketType>::size,
-+        alignedSize = (int(size) / packetSize) * packetSize
-+      };
-+
-+      copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
-+      copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
-+    }
-   }
- };
- 
-@@ -465,9 +473,17 @@ struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, NoUnrolling> {
- 
- template <typename Kernel>
- struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, CompleteUnrolling> {
--  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
--    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
--    copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
-+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
-+    if (internal::is_constant_evaluated()) {
-+      for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
-+        for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
-+          kernel.assignCoeffByOuterInner(outer, inner);
-+        }
-+      }
-+    } else {
-+      typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
-+      copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
-+    }
-   }
- };
- 
-@@ -498,8 +514,16 @@ struct dense_assignment_loop<Kernel, LinearTraversal, NoUnrolling> {
- template <typename Kernel>
- struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
-   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
--    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
--    copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
-+    if (internal::is_constant_evaluated()) {
-+      for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
-+        for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
-+          kernel.assignCoeffByOuterInner(outer, inner);
-+        }
-+      }
-+    } else {
-+      typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
-+      copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
-+    }
-   }
- };
- 
-@@ -510,41 +534,49 @@ struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
- template <typename Kernel>
- struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, NoUnrolling> {
-   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
--    typedef typename Kernel::Scalar Scalar;
--    typedef typename Kernel::PacketType PacketType;
--    enum {
--      packetSize = unpacket_traits<PacketType>::size,
--      requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
--      alignable =
--          packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
--      dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
--      dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
--    };
--    const Scalar* dst_ptr = kernel.dstDataPtr();
--    if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
--      // the pointer is not aligned-on scalar, so alignment is not possible
--      return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
--    }
--    const Index packetAlignedMask = packetSize - 1;
--    const Index innerSize = kernel.innerSize();
--    const Index outerSize = kernel.outerSize();
--    const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
--    Index alignedStart =
--        ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
--
--    for (Index outer = 0; outer < outerSize; ++outer) {
--      const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
--      // do the non-vectorizable part of the assignment
--      for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
--
--      // do the vectorizable part of the assignment
--      for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
--        kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
--
--      // do the non-vectorizable part of the assignment
--      for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
--
--      alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
-+    if (internal::is_constant_evaluated()) {
-+      for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
-+        for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
-+          kernel.assignCoeffByOuterInner(outer, inner);
-+        }
-+      }
-+    } else {
-+      typedef typename Kernel::Scalar Scalar;
-+      typedef typename Kernel::PacketType PacketType;
-+      enum {
-+        packetSize = unpacket_traits<PacketType>::size,
-+        requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
-+        alignable =
-+            packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
-+        dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
-+        dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
-+      };
-+      const Scalar* dst_ptr = kernel.dstDataPtr();
-+      if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
-+        // the pointer is not aligned-on scalar, so alignment is not possible
-+        return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
-+      }
-+      const Index packetAlignedMask = packetSize - 1;
-+      const Index innerSize = kernel.innerSize();
-+      const Index outerSize = kernel.outerSize();
-+      const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
-+      Index alignedStart =
-+          ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
-+
-+      for (Index outer = 0; outer < outerSize; ++outer) {
-+        const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
-+        // do the non-vectorizable part of the assignment
-+        for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
-+
-+        // do the vectorizable part of the assignment
-+        for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
-+          kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
-+
-+        // do the non-vectorizable part of the assignment
-+        for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
-+
-+        alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
-+      }
-     }
-   }
- };
-@@ -594,9 +626,9 @@ class generic_dense_assignment_kernel {
-   typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor> AssignmentTraits;
-   typedef typename AssignmentTraits::PacketType PacketType;
- 
--  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst,
--                                                                        const SrcEvaluatorType& src,
--                                                                        const Functor& func, DstXprType& dstExpr)
-+  EIGEN_DEVICE_FUNC
-+  EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src,
-+                                                                const Functor& func, DstXprType& dstExpr)
-       : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) {
- #ifdef EIGEN_DEBUG_ASSIGN
-     AssignmentTraits::debug();
-@@ -614,7 +646,7 @@ class generic_dense_assignment_kernel {
-   EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; }
- 
-   /// Assign src(row,col) to dst(row,col) through the assignment functor.
--  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) {
-+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(Index row, Index col) {
-     m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col));
-   }
- 
-@@ -624,7 +656,7 @@ class generic_dense_assignment_kernel {
-   }
- 
-   /// \sa assignCoeff(Index,Index)
--  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) {
-+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeffByOuterInner(Index outer, Index inner) {
-     Index row = rowIndexByOuterInner(outer, inner);
-     Index col = colIndexByOuterInner(outer, inner);
-     assignCoeff(row, col);
-@@ -648,7 +680,7 @@ class generic_dense_assignment_kernel {
-     assignPacket<StoreMode, LoadMode, Packet>(row, col);
-   }
- 
--  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) {
-+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index rowIndexByOuterInner(Index outer, Index inner) {
-     typedef typename DstEvaluatorType::ExpressionTraits Traits;
-     return int(Traits::RowsAtCompileTime) == 1          ? 0
-            : int(Traits::ColsAtCompileTime) == 1        ? inner
-@@ -656,7 +688,7 @@ class generic_dense_assignment_kernel {
-                                                         : inner;
-   }
- 
--  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) {
-+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index colIndexByOuterInner(Index outer, Index inner) {
-     typedef typename DstEvaluatorType::ExpressionTraits Traits;
-     return int(Traits::ColsAtCompileTime) == 1          ? 0
-            : int(Traits::RowsAtCompileTime) == 1        ? inner
-@@ -708,8 +740,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, co
- }
- 
- template <typename DstXprType, typename SrcXprType, typename T1, typename T2>
--EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
--                                                             const internal::assign_op<T1, T2>& /*func*/) {
-+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
-+                                                                       const internal::assign_op<T1, T2>& /*func*/) {
-   Index dstRows = src.rows();
-   Index dstCols = src.cols();
-   if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols);
-@@ -790,7 +822,7 @@ struct Assignment;
- // not has to bother about these annoying details.
- 
- template <typename Dst, typename Src>
--EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) {
-+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(Dst& dst, const Src& src) {
-   call_assignment(dst, src, internal::assign_op<typename Dst::Scalar, typename Src::Scalar>());
- }
- template <typename Dst, typename Src>
-@@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment(
- }
- 
- template <typename Dst, typename Src, typename Func>
--EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(
-+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(
-     Dst& dst, const Src& src, const Func& func, std::enable_if_t<!evaluator_assume_aliasing<Src>::value, void*> = 0) {
-   call_assignment_no_alias(dst, src, func);
- }
-@@ -891,9 +923,12 @@ EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src);
- // both partial specialization+SFINAE without ambiguous specialization
- template <typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
- struct Assignment<DstXprType, SrcXprType, Functor, Dense2Dense, Weak> {
--  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) {
-+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(DstXprType& dst, const SrcXprType& src,
-+                                                                  const Functor& func) {
- #ifndef EIGEN_NO_DEBUG
--    internal::check_for_aliasing(dst, src);
-+    if (!internal::is_constant_evaluated()) {
-+      internal::check_for_aliasing(dst, src);
-+    }
- #endif
- 
-     call_dense_assignment_loop(dst, src, func);
-diff --git a/Eigen/src/Core/EigenBase.h b/Eigen/src/Core/EigenBase.h
-index 6d167006a094181fa3693b19f6b9daeb6f2afb79..894bfc13b15eb994abd90f100da15de5bd8b22b7 100644
---- a/Eigen/src/Core/EigenBase.h
-+++ b/Eigen/src/Core/EigenBase.h
-@@ -50,7 +50,7 @@ struct EigenBase {
-   /** \returns a const reference to the derived object */
-   EIGEN_DEVICE_FUNC constexpr const Derived& derived() const { return *static_cast<const Derived*>(this); }
- 
--  EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const {
-+  EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const {
-     return *static_cast<Derived*>(const_cast<EigenBase*>(this));
-   }
-   EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast<const Derived*>(this); }
-diff --git a/Eigen/src/Core/functors/AssignmentFunctors.h b/Eigen/src/Core/functors/AssignmentFunctors.h
-index 09d1da8ca2bcb41384520f46e2b793ba8b28a798..3687bb20db4dfe1a2f6cf1342b4fcbd8f91f1f68 100644
---- a/Eigen/src/Core/functors/AssignmentFunctors.h
-+++ b/Eigen/src/Core/functors/AssignmentFunctors.h
-@@ -23,7 +23,7 @@ namespace internal {
-  */
- template <typename DstScalar, typename SrcScalar>
- struct assign_op {
--  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
-+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
- 
-   template <int Alignment, typename Packet>
-   EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {

upstream_utils/expected.py

@@ -1,18 +1,16 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
 from upstream_utils import Lib
 
 
-def copy_upstream_src(wpilib_root):
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    wpiutil = wpilib_root / "wpiutil"
 
     # Copy expected header into allwpilib
-    dest_filename = os.path.join(
-        wpiutil, "src/main/native/thirdparty/expected/include/wpi/expected"
-    )
+    dest_filename = wpiutil / "src/main/native/thirdparty/expected/include/wpi/expected"
     shutil.copyfile("include/tl/expected.hpp", dest_filename)
 
     # Rename namespace from tl to wpi, and detail to detail_expected

upstream_utils/fmt.py

@@ -1,33 +1,33 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, walk_cwd_and_copy_if
+from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    wpiutil = wpilib_root / "wpiutil"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/fmtlib/src",
         "src/main/native/thirdparty/fmtlib/include",
     ]:
-        shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True)
+        shutil.rmtree(wpiutil / d, ignore_errors=True)
 
     # Copy fmt source files into allwpilib
     walk_cwd_and_copy_if(
-        lambda dp, f: dp.startswith(os.path.join(".", "src"))
+        lambda dp, f: has_prefix(dp, Path("src"))
         and f.endswith(".cc")
         and f != "fmt.cc",
-        os.path.join(wpiutil, "src/main/native/thirdparty/fmtlib"),
+        wpiutil / "src/main/native/thirdparty/fmtlib",
     )
 
     # Copy fmt header files into allwpilib
     walk_cwd_and_copy_if(
-        lambda dp, f: dp.startswith(os.path.join(".", "include", "fmt")),
-        os.path.join(wpiutil, "src/main/native/thirdparty/fmtlib"),
+        lambda dp, f: has_prefix(dp, Path("include/fmt")),
+        wpiutil / "src/main/native/thirdparty/fmtlib",
     )
 
 

upstream_utils/gcem.py

@@ -1,24 +1,24 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, walk_cwd_and_copy_if
+from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    wpimath = os.path.join(wpilib_root, "wpimath")
+def copy_upstream_src(wpilib_root: Path):
+    wpimath = wpilib_root / "wpimath"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/gcem/include",
     ]:
-        shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True)
+        shutil.rmtree(wpimath / d, ignore_errors=True)
 
     # Copy gcem include files into allwpilib
     walk_cwd_and_copy_if(
-        lambda dp, f: dp.startswith(os.path.join(".", "include")),
-        os.path.join(wpimath, "src/main/native/thirdparty/gcem"),
+        lambda dp, f: has_prefix(dp, Path("include")),
+        wpimath / "src/main/native/thirdparty/gcem",
     )
 
 

upstream_utils/gl3w.py

@@ -1,16 +1,16 @@
 #!/usr/bin/env python3
 
-import os
+from pathlib import Path
 
 from upstream_utils import Lib, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    gl3w = os.path.join(wpilib_root, "thirdparty", "imgui_suite", "gl3w")
+def copy_upstream_src(wpilib_root: Path):
+    gl3w = wpilib_root / "thirdparty/imgui_suite/gl3w"
 
     walk_cwd_and_copy_if(
         lambda dp, f: f == "gl3w_gen.py",
-        os.path.join(gl3w),
+        gl3w,
     )
 
 

upstream_utils/glfw.py

@@ -1,57 +1,47 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, walk_cwd_and_copy_if
+from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if
 
 
-def matches(dp, f, allowed_files):
-    path = os.path.join(dp, f)
-    return path in allowed_files
-
-
-def walk_and_copy_if_matches(allowed_files, output_directory):
-    walk_cwd_and_copy_if(
-        lambda dp, f: matches(dp, f, allowed_files),
-        output_directory,
-    )
-
-
-def copy_upstream_src(wpilib_root):
-    glfw = os.path.join(wpilib_root, "thirdparty", "imgui_suite", "glfw")
+def copy_upstream_src(wpilib_root: Path):
+    glfw = wpilib_root / "thirdparty/imgui_suite/glfw"
 
     # Delete old install
     for d in ["include", "src", "CMake"]:
-        shutil.rmtree(os.path.join(glfw, d), ignore_errors=True)
+        shutil.rmtree(glfw / d, ignore_errors=True)
 
     hdr_allow_list = [
-        "./include/GLFW/glfw3.h",
-        "./include/GLFW/glfw3native.h",
+        Path("include/GLFW/glfw3.h"),
+        Path("include/GLFW/glfw3native.h"),
     ]
 
-    walk_and_copy_if_matches(hdr_allow_list, glfw)
+    walk_cwd_and_copy_if(
+        lambda dp, f: dp / f in hdr_allow_list,
+        glfw,
+    )
 
-    def src_filter(dp, f):
-        if f.endswith("CMakeLists.txt"):
+    def src_filter(dp: Path, f: str):
+        if f == "CMakeLists.txt":
             return False
 
-        if dp.startswith(os.path.join(".", "src")):
+        if has_prefix(dp, Path("src")):
             return True
 
         return False
 
     src_files = walk_cwd_and_copy_if(
         src_filter,
-        os.path.join(glfw),
+        glfw,
     )
 
-    def cmake_filter(dp, f):
-        if dp.startswith(os.path.join(".", "CMake")):
+    def cmake_filter(dp: Path, f: str):
+        if has_prefix(dp, Path("CMake")):
             return True
 
-        path = os.path.join(dp, f)
-        if path in ["./src/CMakeLists.txt", "./CMakeLists.txt"]:
+        if dp / f in [Path("src/CMakeLists.txt"), Path("CMakeLists.txt")]:
             return True
 
         return False
@@ -59,7 +49,7 @@ def copy_upstream_src(wpilib_root):
     # Copy CMAKE files
     walk_cwd_and_copy_if(
         cmake_filter,
-        os.path.join(glfw),
+        glfw,
     )
 
 

upstream_utils/googletest.py

@@ -2,8 +2,9 @@
 
 import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, walk_cwd_and_copy_if, walk_if
+from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if
 
 EXCLUDED_FILES = [
     "gtest_main.cc",
@@ -14,49 +15,39 @@ EXCLUDED_FILES = [
 ]
 
 
-def walk_and_remap_copy(third_party_root, include_prefix):
-    gmock_files = walk_if(
-        ".", lambda dp, f: include_prefix in dp and f not in EXCLUDED_FILES
-    )
-
-    for f in gmock_files:
-        dst_file = os.path.join(
-            third_party_root, "include", f[len(include_prefix) + 1 :]
-        )
-        dest_dir = os.path.dirname(dst_file)
-        if not os.path.exists(dest_dir):
-            os.makedirs(dest_dir)
-        shutil.copyfile(f, dst_file)
-
-
-def copy_upstream_src(wpilib_root):
-    third_party_root = os.path.join(wpilib_root, "thirdparty/googletest")
+def copy_upstream_src(wpilib_root: Path):
+    upstream_root = Path(".").absolute()
+    third_party_root = wpilib_root / "thirdparty/googletest"
 
     # Delete old install
     for d in [
         "include",
         "src",
     ]:
-        shutil.rmtree(os.path.join(third_party_root, d), ignore_errors=True)
+        shutil.rmtree(third_party_root / d, ignore_errors=True)
 
     walk_cwd_and_copy_if(
-        lambda dp, f: "googlemock/src" in dp and f not in EXCLUDED_FILES,
-        os.path.join(third_party_root, "src"),
+        lambda dp, f: has_prefix(dp, Path("googlemock/src"))
+        and f not in EXCLUDED_FILES,
+        third_party_root / "src",
     )
 
     walk_cwd_and_copy_if(
-        lambda dp, f: "googletest/src" in dp and f not in EXCLUDED_FILES,
-        os.path.join(third_party_root, "src"),
+        lambda dp, f: has_prefix(dp, Path("googletest/src"))
+        and f not in EXCLUDED_FILES,
+        third_party_root / "src",
     )
 
-    walk_and_remap_copy(
-        third_party_root,
-        "./googlemock/include",
+    os.chdir(upstream_root / "googlemock/include")
+    walk_cwd_and_copy_if(
+        lambda dp, f: f not in EXCLUDED_FILES,
+        third_party_root / "include",
     )
 
-    walk_and_remap_copy(
-        third_party_root,
-        "./googletest/include",
+    os.chdir(upstream_root / "googletest/include")
+    walk_cwd_and_copy_if(
+        lambda dp, f: f not in EXCLUDED_FILES,
+        third_party_root / "include",
     )
 
 

upstream_utils/imgui.py

@@ -1,62 +1,57 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
 from upstream_utils import Lib, walk_cwd_and_copy_if
 
 
-def matches(dp, f, allowed_files):
-    path = os.path.join(dp, f)
-    return path in allowed_files
-
-
-def walk_and_copy_if_matches(allowed_files, output_directory):
+def walk_and_copy_if_matches(allowed_files: list[Path], output_directory: Path):
     walk_cwd_and_copy_if(
-        lambda dp, f: matches(dp, f, allowed_files),
+        lambda dp, f: dp / f in allowed_files,
         output_directory,
     )
 
 
-def copy_upstream_src(wpilib_root):
-    imgui = os.path.join(wpilib_root, "thirdparty", "imgui_suite", "imgui")
+def copy_upstream_src(wpilib_root: Path):
+    imgui = wpilib_root / "thirdparty/imgui_suite/imgui"
 
     # Delete old install
     for d in ["include", "cpp"]:
-        shutil.rmtree(os.path.join(imgui, d), ignore_errors=True)
+        shutil.rmtree(imgui / d, ignore_errors=True)
 
     hdr_allow_list = [
-        "./imgui.h",
-        "./imstb_truetype.h",
-        "./imgui_internal.h",
-        "./imstb_rectpack.h",
-        "./imconfig.h",
-        "./imstb_textedit.h",
-        "./backends/imgui_impl_glfw.h",
-        "./backends/imgui_impl_metal.h",
-        "./backends/imgui_impl_opengl3.h",
-        "./backends/imgui_impl_dx11.h",
-        "./backends/imgui_impl_opengl3_loader.h",
-        "./backends/imgui_impl_opengl2.h",
-        "./misc/cpp/imgui_stdlib.h",
+        Path("imgui.h"),
+        Path("imstb_truetype.h"),
+        Path("imgui_internal.h"),
+        Path("imstb_rectpack.h"),
+        Path("imconfig.h"),
+        Path("imstb_textedit.h"),
+        Path("backends/imgui_impl_glfw.h"),
+        Path("backends/imgui_impl_metal.h"),
+        Path("backends/imgui_impl_opengl3.h"),
+        Path("backends/imgui_impl_dx11.h"),
+        Path("backends/imgui_impl_opengl3_loader.h"),
+        Path("backends/imgui_impl_opengl2.h"),
+        Path("misc/cpp/imgui_stdlib.h"),
     ]
 
     src_allow_list = [
-        "./backends/imgui_impl_dx11.cpp",
-        "./backends/imgui_impl_glfw.cpp",
-        "./backends/imgui_impl_metal.mm",
-        "./backends/imgui_impl_opengl2.cpp",
-        "./backends/imgui_impl_opengl3.cpp",
-        "./imgui.cpp",
-        "./imgui_demo.cpp",
-        "./imgui_draw.cpp",
-        "./imgui_tables.cpp",
-        "./imgui_widgets.cpp",
-        "./misc/cpp/imgui_stdlib.cpp",
+        Path("backends/imgui_impl_dx11.cpp"),
+        Path("backends/imgui_impl_glfw.cpp"),
+        Path("backends/imgui_impl_metal.mm"),
+        Path("backends/imgui_impl_opengl2.cpp"),
+        Path("backends/imgui_impl_opengl3.cpp"),
+        Path("imgui.cpp"),
+        Path("imgui_demo.cpp"),
+        Path("imgui_draw.cpp"),
+        Path("imgui_tables.cpp"),
+        Path("imgui_widgets.cpp"),
+        Path("misc/cpp/imgui_stdlib.cpp"),
     ]
 
-    walk_and_copy_if_matches(hdr_allow_list, os.path.join(imgui, "include"))
-    walk_and_copy_if_matches(src_allow_list, os.path.join(imgui, "cpp"))
+    walk_and_copy_if_matches(hdr_allow_list, imgui / "include")
+    walk_and_copy_if_matches(src_allow_list, imgui / "cpp")
 
 
 def main():

upstream_utils/imgui_patches/0001-Set-IMGUI_DEFINE_MATH_OPERATORS.patch

@@ -0,0 +1,23 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Jade Turner <spacey-sooty@proton.me>
+Date: Fri, 11 Apr 2025 11:20:41 +0800
+Subject: [PATCH] Set IMGUI_DEFINE_MATH_OPERATORS
+
+Signed-off-by: Jade Turner <spacey-sooty@proton.me>
+---
+ imconfig.h | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/imconfig.h b/imconfig.h
+index b56ba49467ebb388e038f0a23aed46044085bdff..291ce1995412f10e784beaf3f4c51e02d90b9813 100644
+--- a/imconfig.h
++++ b/imconfig.h
+@@ -98,7 +98,7 @@
+         operator MyVec4() const { return MyVec4(x,y,z,w); }
+ */
+ //---- ...Or use Dear ImGui's own very basic math operators.
+-//#define IMGUI_DEFINE_MATH_OPERATORS
++#define IMGUI_DEFINE_MATH_OPERATORS
+ 
+ //---- Use 32-bit vertex indices (default is 16-bit) is one way to allow large meshes with more than 64K vertices.
+ // Your renderer backend will need to support it (most example renderer backends support both 16/32-bit indices).

upstream_utils/implot.py

@@ -1,37 +1,27 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
 from upstream_utils import Lib, walk_cwd_and_copy_if
 
 
-def matches(dp, f, allowed_files):
-    path = os.path.join(dp, f)
-    return path in allowed_files
-
-
-def walk_and_copy_if_matches(allowed_files, output_directory):
-    walk_cwd_and_copy_if(
-        lambda dp, f: matches(dp, f, allowed_files),
-        output_directory,
-    )
-
-
-def copy_upstream_src(wpilib_root):
-    implot = os.path.join(wpilib_root, "thirdparty", "imgui_suite", "implot")
+def copy_upstream_src(wpilib_root: Path):
+    implot = wpilib_root / "thirdparty/imgui_suite/implot"
 
     # Delete old install
     for d in ["include", "cpp"]:
-        shutil.rmtree(os.path.join(implot, d), ignore_errors=True)
+        shutil.rmtree(implot / d, ignore_errors=True)
 
     # Copy files
     walk_cwd_and_copy_if(
         lambda dp, f: f.endswith(".h"),
-        os.path.join(implot, "include"),
+        implot / "include",
     )
-    walk_and_copy_if_matches(
-        ["./implot_items.cpp", "./implot.cpp"], os.path.join(implot, "cpp")
+    implot_files = [Path("implot_items.cpp"), Path("implot.cpp")]
+    walk_cwd_and_copy_if(
+        lambda dp, f: dp / f in implot_files,
+        implot / "cpp",
     )
 
 

upstream_utils/json.py

@@ -2,35 +2,32 @@
 
 import os
 import shutil
+from pathlib import Path
 
 from upstream_utils import Lib, walk_if
 
 
-def copy_upstream_src(wpilib_root):
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    wpiutil = wpilib_root / "wpiutil"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/json/include",
     ]:
-        shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True)
+        shutil.rmtree(wpiutil / d, ignore_errors=True)
 
     # Create lists of source and destination files
     os.chdir("include/nlohmann")
-    files = walk_if(".", lambda dp, f: True)
-    src_include_files = [os.path.abspath(f) for f in files]
-    wpiutil_json_root = os.path.join(
-        wpiutil, "src/main/native/thirdparty/json/include/wpi"
-    )
-    dest_include_files = [
-        os.path.join(wpiutil_json_root, f.replace(".hpp", ".h")) for f in files
-    ]
+    files = walk_if(Path("."), lambda dp, f: True)
+    src_include_files = [f.absolute() for f in files]
+    wpiutil_json_root = wpiutil / "src/main/native/thirdparty/json/include/wpi"
+    dest_include_files = [wpiutil_json_root / f.with_suffix(".h") for f in files]
 
     # Copy json header files into allwpilib
     for i in range(len(src_include_files)):
-        dest_dir = os.path.dirname(dest_include_files[i])
-        if not os.path.exists(dest_dir):
-            os.makedirs(dest_dir)
+        dest_dir = dest_include_files[i].parent
+        if not dest_dir.exists():
+            dest_dir.mkdir(parents=True)
         shutil.copyfile(src_include_files[i], dest_include_files[i])
 
     for include_file in dest_include_files:

upstream_utils/libdogleg.py

@@ -1,24 +1,24 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
 from upstream_utils import Lib, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    wpical = os.path.join(wpilib_root, "wpical")
+def copy_upstream_src(wpilib_root: Path):
+    wpical = wpilib_root / "wpical"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/libdogleg/src",
         "src/main/native/thirdparty/libdogleg/include",
     ]:
-        shutil.rmtree(os.path.join(wpical, d), ignore_errors=True)
+        shutil.rmtree(wpical / d, ignore_errors=True)
 
     files = walk_cwd_and_copy_if(
-        lambda dp, f: f.endswith("dogleg.h"),
-        os.path.join(wpical, "src/main/native/thirdparty/libdogleg/include"),
+        lambda dp, f: f == "dogleg.h",
+        wpical / "src/main/native/thirdparty/libdogleg/include",
     )
     for f in files:
         with open(f) as file:
@@ -30,8 +30,8 @@ def copy_upstream_src(wpilib_root):
             file.write(content)
 
     files = walk_cwd_and_copy_if(
-        lambda dp, f: f.endswith("dogleg.cpp"),
-        os.path.join(wpical, "src/main/native/thirdparty/libdogleg/src"),
+        lambda dp, f: f == "dogleg.cpp",
+        wpical / "src/main/native/thirdparty/libdogleg/src",
     )
     for f in files:
         with open(f) as file:

upstream_utils/libuv.py

@@ -1,17 +1,17 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, walk_cwd_and_copy_if
+from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    wpinet = os.path.join(wpilib_root, "wpinet")
+def copy_upstream_src(wpilib_root: Path):
+    wpinet = wpilib_root / "wpinet"
 
     # Delete old install
     for d in ["src/main/native/thirdparty/libuv"]:
-        shutil.rmtree(os.path.join(wpinet, d), ignore_errors=True)
+        shutil.rmtree(wpinet / d, ignore_errors=True)
 
     include_ignorelist = [
         "aix.h",
@@ -21,9 +21,8 @@ def copy_upstream_src(wpilib_root):
     ]
 
     walk_cwd_and_copy_if(
-        lambda dp, f: dp.startswith(os.path.join(".", "include"))
-        and f not in include_ignorelist,
-        os.path.join(wpinet, "src/main/native/thirdparty/libuv"),
+        lambda dp, f: has_prefix(dp, Path("include")) and f not in include_ignorelist,
+        wpinet / "src/main/native/thirdparty/libuv",
     )
 
     src_ignorelist = [
@@ -43,9 +42,8 @@ def copy_upstream_src(wpilib_root):
         "sysinfo-memory.c",
     ]
     walk_cwd_and_copy_if(
-        lambda dp, f: dp.startswith(os.path.join(".", "src"))
-        and f not in src_ignorelist,
-        os.path.join(wpinet, "src/main/native/thirdparty/libuv"),
+        lambda dp, f: has_prefix(dp, Path("src")) and f not in src_ignorelist,
+        wpinet / "src/main/native/thirdparty/libuv",
         rename_c_to_cpp=True,
     )
 

upstream_utils/llvm.py

@@ -1,12 +1,12 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
 from upstream_utils import Lib
 
 
-def run_global_replacements(wpiutil_llvm_files):
+def run_global_replacements(wpiutil_llvm_files: list[Path]):
     for wpi_file in wpiutil_llvm_files:
         with open(wpi_file) as f:
             content = f.read()
@@ -23,7 +23,7 @@ def run_global_replacements(wpiutil_llvm_files):
         # Fix uses of span
         content = content.replace("span", "std::span")
         content = content.replace("include <std::span>", "include <span>")
-        if wpi_file.endswith("ConvertUTFWrapper.cpp"):
+        if wpi_file.name == "ConvertUTFWrapper.cpp":
             content = content.replace(
                 "const UTF16 *Src = reinterpret_cast<const UTF16 *>(SrcBytes.begin());",
                 "const UTF16 *Src = reinterpret_cast<const UTF16 *>(&*SrcBytes.begin());",
@@ -88,19 +88,19 @@ def run_global_replacements(wpiutil_llvm_files):
             f.write(content)
 
 
-def flattened_llvm_files(llvm, dirs_to_keep):
-    file_lookup = {}
+def flattened_llvm_files(llvm: Path, dirs_to_keep: list[Path]):
+    file_lookup: dict[str, Path] = {}
 
     for dir_to_keep in dirs_to_keep:
-        dir_to_crawl = os.path.join(llvm, dir_to_keep)
-        for root, _, files in os.walk(dir_to_crawl):
+        dir_to_crawl = llvm / dir_to_keep
+        for root, _, files in dir_to_crawl.walk():
             for f in files:
-                file_lookup[f] = os.path.join(root, f)
+                file_lookup[f] = root / f
 
     return file_lookup
 
 
-def find_wpiutil_llvm_files(wpiutil_root, subfolder):
+def find_wpiutil_llvm_files(wpiutil_root: Path, subfolder: str):
     # These files have substantial changes, not worth managing with the patching process
     ignore_list = [
         "StringExtras.h",
@@ -110,22 +110,22 @@ def find_wpiutil_llvm_files(wpiutil_root, subfolder):
         "SmallVectorMemoryBuffer.h",
     ]
 
-    wpiutil_files = []
-    for root, _, files in os.walk(os.path.join(wpiutil_root, subfolder)):
+    wpiutil_files: list[Path] = []
+    for root, _, files in (wpiutil_root / subfolder).walk():
         for f in files:
             if f not in ignore_list:
-                full_file = os.path.join(root, f)
+                full_file = root / f
                 wpiutil_files.append(full_file)
 
     return wpiutil_files
 
 
-def overwrite_files(wpiutil_files, llvm_files):
+def overwrite_files(wpiutil_files: list[Path], llvm_files: dict[str, Path]):
     # Very sparse rips from LLVM sources. Not worth tyring to make match upstream
     unmatched_files_whitelist = ["fs.h", "fs.cpp", "function_ref.h"]
 
     for wpi_file in wpiutil_files:
-        wpi_base_name = os.path.basename(wpi_file)
+        wpi_base_name = wpi_file.name
         if wpi_base_name in llvm_files:
             shutil.copyfile(llvm_files[wpi_base_name], wpi_file)
 
@@ -133,14 +133,14 @@ def overwrite_files(wpiutil_files, llvm_files):
             print(f"No file match for {wpi_file}, check if LLVM deleted it")
 
 
-def overwrite_source(wpiutil_root, llvm_root):
+def overwrite_source(wpiutil_root: Path, llvm_root: Path):
     llvm_files = flattened_llvm_files(
         llvm_root,
         [
-            "llvm/include/llvm/ADT/",
-            "llvm/include/llvm/Config",
-            "llvm/include/llvm/Support/",
-            "llvm/lib/Support/",
+            Path("llvm/include/llvm/ADT/"),
+            Path("llvm/include/llvm/Config/"),
+            Path("llvm/include/llvm/Support/"),
+            Path("llvm/lib/Support/"),
         ],
     )
     wpi_files = find_wpiutil_llvm_files(
@@ -153,10 +153,14 @@ def overwrite_source(wpiutil_root, llvm_root):
     run_global_replacements(wpi_files)
 
 
-def overwrite_tests(wpiutil_root, llvm_root):
+def overwrite_tests(wpiutil_root: Path, llvm_root: Path):
     llvm_files = flattened_llvm_files(
         llvm_root,
-        ["llvm/unittests/ADT/", "llvm/unittests/Config", "llvm/unittests/Support/"],
+        [
+            Path("llvm/unittests/ADT/"),
+            Path("llvm/unittests/Config/"),
+            Path("llvm/unittests/Support/"),
+        ],
     )
     wpi_files = find_wpiutil_llvm_files(wpiutil_root, "src/test/native/cpp/llvm")
 
@@ -164,9 +168,9 @@ def overwrite_tests(wpiutil_root, llvm_root):
     run_global_replacements(wpi_files)
 
 
-def copy_upstream_src(wpilib_root):
-    upstream_root = os.path.abspath(".")
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    upstream_root = Path(".").absolute()
+    wpiutil = wpilib_root / "wpiutil"
 
     overwrite_source(wpiutil, upstream_root)
     overwrite_tests(wpiutil, upstream_root)

upstream_utils/memory.py

@@ -2,11 +2,12 @@
 
 import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, copy_to, walk_if
+from upstream_utils import Lib, walk_cwd_and_copy_if
 
 
-def run_source_replacements(memory_files):
+def run_source_replacements(memory_files: list[Path]):
     for wpi_file in memory_files:
         with open(wpi_file) as f:
             content = f.read()
@@ -21,9 +22,9 @@ def run_source_replacements(memory_files):
             f.write(content)
 
 
-def run_header_replacements(memory_files):
+def run_header_replacements(memory_files: list[Path]):
     for wpi_file in memory_files:
-        if "detail" not in wpi_file:
+        if "detail" not in wpi_file.parts:
             continue
         with open(wpi_file) as f:
             content = f.read()
@@ -35,7 +36,7 @@ def run_header_replacements(memory_files):
             f.write(content)
 
 
-def run_global_replacements(memory_files):
+def run_global_replacements(memory_files: list[Path]):
     for wpi_file in memory_files:
         with open(wpi_file) as f:
             content = f.read()
@@ -52,42 +53,41 @@ def run_global_replacements(memory_files):
             f.write(content)
 
 
-def copy_upstream_src(wpilib_root):
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    upstream_root = Path(".").absolute()
+    wpiutil = wpilib_root / "wpiutil"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/memory/src",
         "src/main/native/thirdparty/memory/include",
     ]:
-        shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True)
+        shutil.rmtree(wpiutil / d, ignore_errors=True)
 
     # Copy sources
-    src_files = walk_if("src", lambda dp, f: f.endswith(".cpp") or f.endswith(".hpp"))
-    src_files = copy_to(
-        src_files, os.path.join(wpiutil, "src/main/native/thirdparty/memory")
+    os.chdir(upstream_root / "src")
+    src_files = walk_cwd_and_copy_if(
+        lambda dp, f: f.endswith(".cpp") or f.endswith(".hpp"),
+        wpiutil / "src/main/native/thirdparty/memory/src",
     )
     run_global_replacements(src_files)
     run_source_replacements(src_files)
 
     # Copy headers
-    os.chdir(os.path.join("include", "foonathan"))
-    include_files = walk_if(".", lambda dp, f: f.endswith(".hpp"))
-    include_files = copy_to(
-        include_files,
-        os.path.join(wpiutil, "src/main/native/thirdparty/memory/include/wpi"),
+    os.chdir(upstream_root / "include/foonathan")
+    include_files = walk_cwd_and_copy_if(
+        lambda dp, f: f.endswith(".hpp"),
+        wpiutil / "src/main/native/thirdparty/memory/include/wpi",
     )
-    os.chdir(os.path.join("..", ".."))
+    os.chdir(upstream_root)
     run_global_replacements(include_files)
     run_header_replacements(include_files)
 
     # Copy config_impl.hpp
     shutil.copyfile(
-        os.path.join(wpilib_root, "upstream_utils/memory_files/config_impl.hpp"),
-        os.path.join(
-            wpiutil,
-            "src/main/native/thirdparty/memory/include/wpi/memory/config_impl.hpp",
-        ),
+        wpilib_root / "upstream_utils/memory_files/config_impl.hpp",
+        wpiutil
+        / "src/main/native/thirdparty/memory/include/wpi/memory/config_impl.hpp",
     )
 
 

upstream_utils/mpack.py

@@ -3,36 +3,34 @@
 import os
 import shutil
 import subprocess
+from pathlib import Path
 
 from upstream_utils import Lib, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    wpiutil = wpilib_root / "wpiutil"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/mpack/src",
         "src/main/native/thirdparty/mpack/include",
     ]:
-        shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True)
+        shutil.rmtree(wpiutil / d, ignore_errors=True)
 
     # Run the amalgmation script
     subprocess.check_call(["bash", "tools/amalgamate.sh"])
 
     # Copy the files
-    amalgamation_source_dir = os.path.join(
-        ".", ".build", "amalgamation", "src", "mpack"
-    )
-    os.chdir(amalgamation_source_dir)
+    os.chdir(Path(".build/amalgamation/src/mpack"))
 
     walk_cwd_and_copy_if(
         lambda dp, f: f.endswith(".h"),
-        os.path.join(wpiutil, "src/main/native/thirdparty/mpack/include/wpi"),
+        wpiutil / "src/main/native/thirdparty/mpack/include/wpi",
     )
     walk_cwd_and_copy_if(
         lambda dp, f: f.endswith(".c"),
-        os.path.join(wpiutil, "src/main/native/thirdparty/mpack/src"),
+        wpiutil / "src/main/native/thirdparty/mpack/src",
         rename_c_to_cpp=True,
     )
 

upstream_utils/mrcal.py

@@ -1,27 +1,27 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, walk_cwd_and_copy_if
+from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    wpical = os.path.join(wpilib_root, "wpical")
+def copy_upstream_src(wpilib_root: Path):
+    wpical = wpilib_root / "wpical"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/mrcal/src",
         "src/main/native/thirdparty/mrcal/include",
     ]:
-        shutil.rmtree(os.path.join(wpical, d), ignore_errors=True)
+        shutil.rmtree(wpical / d, ignore_errors=True)
 
     files = walk_cwd_and_copy_if(
         lambda dp, f: (f.endswith(".h") or f.endswith(".hh"))
-        and not f.endswith("heap.h")
-        and not f.endswith("stereo-matching-libelas.h")
-        and not dp.startswith(os.path.join(".", "test")),
-        os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"),
+        and not f == "heap.h"
+        and not f == "stereo-matching-libelas.h"
+        and not has_prefix(dp, Path("test")),
+        wpical / "src/main/native/thirdparty/mrcal/include",
     )
     files = files + walk_cwd_and_copy_if(
         lambda dp, f: (
@@ -30,16 +30,16 @@ def copy_upstream_src(wpilib_root):
             or f.endswith(".cpp")
             or f.endswith(".pl")
         )
-        and not f.endswith("heap.cc")
-        and not f.endswith("mrcal-pywrap.c")
-        and not f.endswith("image.c")
-        and not f.endswith("stereo.c")
-        and not f.endswith("stereo-matching-libelas.cc")
-        and not f.endswith("uncertainty.c")
-        and not f.endswith("traverse-sensor-links.c")
-        and not dp.startswith(os.path.join(".", "doc"))
-        and not dp.startswith(os.path.join(".", "test")),
-        os.path.join(wpical, "src/main/native/thirdparty/mrcal/src"),
+        and not f == "heap.cc"
+        and not f == "mrcal-pywrap.c"
+        and not f == "image.c"
+        and not f == "stereo.c"
+        and not f == "stereo-matching-libelas.cc"
+        and not f == "uncertainty.c"
+        and not f == "traverse-sensor-links.c"
+        and not has_prefix(dp, Path("doc"))
+        and not has_prefix(dp, Path("test")),
+        wpical / "src/main/native/thirdparty/mrcal/src",
     )
 
     for f in files:

upstream_utils/mrcal_java.py

@@ -2,46 +2,30 @@
 
 import os
 import shutil
+from pathlib import Path
 
 from upstream_utils import Lib, walk_cwd_and_copy_if
 
 
-def delete_lines_by_range(file_path, start_line, end_line):
-    # Read all lines from the file
-    with open(file_path, "r") as file:
-        lines = file.readlines()
-
-    # Filter out lines that are within the specified range
-    filtered_lines = [
-        line
-        for i, line in enumerate(lines, start=1)
-        if not (start_line <= i <= end_line)
-    ]
-
-    # Write the remaining lines back to the file
-    with open(file_path, "w") as file:
-        file.writelines(filtered_lines)
-
-
-def copy_upstream_src(wpilib_root):
-    wpical = os.path.join(wpilib_root, "wpical")
+def copy_upstream_src(wpilib_root: Path):
+    wpical = wpilib_root / "wpical"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/mrcal_java/src",
         "src/main/native/thirdparty/mrcal_java/include",
     ]:
-        shutil.rmtree(os.path.join(wpical, d), ignore_errors=True)
+        shutil.rmtree(wpical / d, ignore_errors=True)
 
     os.chdir("src")
     files = walk_cwd_and_copy_if(
-        lambda dp, f: f.endswith("mrcal_wrapper.h"),
-        os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/include"),
+        lambda dp, f: f == "mrcal_wrapper.h",
+        wpical / "src/main/native/thirdparty/mrcal_java/include",
     )
 
     files = walk_cwd_and_copy_if(
-        lambda dp, f: f.endswith("mrcal_wrapper.cpp"),
-        os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/src"),
+        lambda dp, f: f == "mrcal_wrapper.cpp",
+        wpical / "src/main/native/thirdparty/mrcal_java/src",
     )
 
     for f in files:

upstream_utils/nanopb.py

@@ -1,39 +1,33 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, copy_to, walk_if
+from upstream_utils import Lib, copy_to, has_prefix, walk_cwd_and_copy_if
 
-nanopb_sources = set(
-    [
-        "pb_encode.c",
-        "pb_decode.c",
-        "pb_common.c",
-    ]
-)
+nanopb_sources = [
+    Path("pb_encode.c"),
+    Path("pb_decode.c"),
+    Path("pb_common.c"),
+]
 
-nanopb_headers = set(
-    [
-        "pb.h",
-        "pb_encode.h",
-        "pb_decode.h",
-        "pb_common.h",
-    ]
-)
+nanopb_headers = [
+    Path("pb.h"),
+    Path("pb_encode.h"),
+    Path("pb_decode.h"),
+    Path("pb_common.h"),
+]
 
-nanopb_generator = set(
-    [
-        "pb.h",
-        "pb_encode.h",
-        "pb_decode.h",
-        "pb_common.h",
-    ]
-)
+nanopb_generator = [
+    Path("pb.h"),
+    Path("pb_encode.h"),
+    Path("pb_decode.h"),
+    Path("pb_common.h"),
+]
 
 
-def copy_upstream_src(wpilib_root):
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    wpiutil = wpilib_root / "wpiutil"
 
     # Delete old install
     for d in [
@@ -41,25 +35,24 @@ def copy_upstream_src(wpilib_root):
         "src/main/native/thirdparty/nanopb/include",
         "src/main/native/thirdparty/nanopb/generator",
     ]:
-        shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True)
+        shutil.rmtree(wpiutil / d, ignore_errors=True)
 
     # Copy nanopb source files into allwpilib
     copy_to(
         nanopb_sources,
-        os.path.join(wpiutil, "src/main/native/thirdparty/nanopb/src"),
+        wpiutil / "src/main/native/thirdparty/nanopb/src",
         rename_c_to_cpp=True,
     )
 
     # Copy nanopb header files into allwpilib
     copy_to(
         nanopb_headers,
-        os.path.join(wpiutil, "src/main/native/thirdparty/nanopb/include"),
+        wpiutil / "src/main/native/thirdparty/nanopb/include",
     )
 
-    generator_files = walk_if("generator", lambda dp, f: True)
-    copy_to(
-        generator_files,
-        os.path.join(wpiutil, "src/main/native/thirdparty/nanopb"),
+    generator_files = walk_cwd_and_copy_if(
+        lambda dp, f: has_prefix(dp, Path("generator")),
+        wpiutil / "src/main/native/thirdparty/nanopb",
     )
 
 

upstream_utils/protobuf.py

@@ -2,237 +2,227 @@
 
 import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, copy_to, walk_if
+from upstream_utils import Lib, walk_cwd_and_copy_if
 
-protobuf_lite_sources = set(
-    [
-        "google/protobuf/any_lite.cc",
-        "google/protobuf/arena.cc",
-        "google/protobuf/arenastring.cc",
-        "google/protobuf/arenaz_sampler.cc",
-        "google/protobuf/extension_set.cc",
-        "google/protobuf/generated_enum_util.cc",
-        "google/protobuf/generated_message_tctable_lite.cc",
-        "google/protobuf/generated_message_util.cc",
-        "google/protobuf/implicit_weak_message.cc",
-        "google/protobuf/inlined_string_field.cc",
-        "google/protobuf/io/coded_stream.cc",
-        "google/protobuf/io/io_win32.cc",
-        "google/protobuf/io/strtod.cc",
-        "google/protobuf/io/zero_copy_stream.cc",
-        "google/protobuf/io/zero_copy_stream_impl.cc",
-        "google/protobuf/io/zero_copy_stream_impl_lite.cc",
-        "google/protobuf/map.cc",
-        "google/protobuf/message_lite.cc",
-        "google/protobuf/parse_context.cc",
-        "google/protobuf/repeated_field.cc",
-        "google/protobuf/repeated_ptr_field.cc",
-        "google/protobuf/stubs/bytestream.cc",
-        "google/protobuf/stubs/common.cc",
-        "google/protobuf/stubs/int128.cc",
-        "google/protobuf/stubs/status.cc",
-        "google/protobuf/stubs/statusor.cc",
-        "google/protobuf/stubs/stringpiece.cc",
-        "google/protobuf/stubs/stringprintf.cc",
-        "google/protobuf/stubs/structurally_valid.cc",
-        "google/protobuf/stubs/strutil.cc",
-        "google/protobuf/stubs/time.cc",
-        "google/protobuf/wire_format_lite.cc",
-    ]
-)
+protobuf_lite_sources = {
+    Path("any_lite.cc"),
+    Path("arena.cc"),
+    Path("arenastring.cc"),
+    Path("arenaz_sampler.cc"),
+    Path("extension_set.cc"),
+    Path("generated_enum_util.cc"),
+    Path("generated_message_tctable_lite.cc"),
+    Path("generated_message_util.cc"),
+    Path("implicit_weak_message.cc"),
+    Path("inlined_string_field.cc"),
+    Path("io/coded_stream.cc"),
+    Path("io/io_win32.cc"),
+    Path("io/strtod.cc"),
+    Path("io/zero_copy_stream.cc"),
+    Path("io/zero_copy_stream_impl.cc"),
+    Path("io/zero_copy_stream_impl_lite.cc"),
+    Path("map.cc"),
+    Path("message_lite.cc"),
+    Path("parse_context.cc"),
+    Path("repeated_field.cc"),
+    Path("repeated_ptr_field.cc"),
+    Path("stubs/bytestream.cc"),
+    Path("stubs/common.cc"),
+    Path("stubs/int128.cc"),
+    Path("stubs/status.cc"),
+    Path("stubs/statusor.cc"),
+    Path("stubs/stringpiece.cc"),
+    Path("stubs/stringprintf.cc"),
+    Path("stubs/structurally_valid.cc"),
+    Path("stubs/strutil.cc"),
+    Path("stubs/time.cc"),
+    Path("wire_format_lite.cc"),
+}
 
-protobuf_lite_includes = set(
-    [
-        "google/protobuf/any.h",
-        "google/protobuf/arena.h",
-        "google/protobuf/arena_impl.h",
-        "google/protobuf/arenastring.h",
-        "google/protobuf/arenaz_sampler.h",
-        "google/protobuf/endian.h",
-        "google/protobuf/explicitly_constructed.h",
-        "google/protobuf/extension_set.h",
-        "google/protobuf/extension_set_inl.h",
-        "google/protobuf/generated_enum_util.h",
-        "google/protobuf/generated_message_tctable_decl.h",
-        "google/protobuf/generated_message_tctable_impl.h",
-        "google/protobuf/generated_message_util.h",
-        "google/protobuf/has_bits.h",
-        "google/protobuf/implicit_weak_message.h",
-        "google/protobuf/inlined_string_field.h",
-        "google/protobuf/io/coded_stream.h",
-        "google/protobuf/io/io_win32.h",
-        "google/protobuf/io/strtod.h",
-        "google/protobuf/io/zero_copy_stream.h",
-        "google/protobuf/io/zero_copy_stream_impl.h",
-        "google/protobuf/io/zero_copy_stream_impl_lite.h",
-        "google/protobuf/map.h",
-        "google/protobuf/map_entry_lite.h",
-        "google/protobuf/map_field_lite.h",
-        "google/protobuf/map_type_handler.h",
-        "google/protobuf/message_lite.h",
-        "google/protobuf/metadata_lite.h",
-        "google/protobuf/parse_context.h",
-        "google/protobuf/port.h",
-        "google/protobuf/repeated_field.h",
-        "google/protobuf/repeated_ptr_field.h",
-        "google/protobuf/stubs/bytestream.h",
-        "google/protobuf/stubs/callback.h",
-        "google/protobuf/stubs/casts.h",
-        "google/protobuf/stubs/common.h",
-        "google/protobuf/stubs/hash.h",
-        "google/protobuf/stubs/logging.h",
-        "google/protobuf/stubs/macros.h",
-        "google/protobuf/stubs/map_util.h",
-        "google/protobuf/stubs/mutex.h",
-        "google/protobuf/stubs/once.h",
-        "google/protobuf/stubs/platform_macros.h",
-        "google/protobuf/stubs/port.h",
-        "google/protobuf/stubs/status.h",
-        "google/protobuf/stubs/stl_util.h",
-        "google/protobuf/stubs/stringpiece.h",
-        "google/protobuf/stubs/strutil.h",
-        "google/protobuf/stubs/template_util.h",
-        "google/protobuf/wire_format_lite.h",
-    ]
-)
+protobuf_lite_includes = {
+    Path("google/protobuf/any.h"),
+    Path("google/protobuf/arena.h"),
+    Path("google/protobuf/arena_impl.h"),
+    Path("google/protobuf/arenastring.h"),
+    Path("google/protobuf/arenaz_sampler.h"),
+    Path("google/protobuf/endian.h"),
+    Path("google/protobuf/explicitly_constructed.h"),
+    Path("google/protobuf/extension_set.h"),
+    Path("google/protobuf/extension_set_inl.h"),
+    Path("google/protobuf/generated_enum_util.h"),
+    Path("google/protobuf/generated_message_tctable_decl.h"),
+    Path("google/protobuf/generated_message_tctable_impl.h"),
+    Path("google/protobuf/generated_message_util.h"),
+    Path("google/protobuf/has_bits.h"),
+    Path("google/protobuf/implicit_weak_message.h"),
+    Path("google/protobuf/inlined_string_field.h"),
+    Path("google/protobuf/io/coded_stream.h"),
+    Path("google/protobuf/io/io_win32.h"),
+    Path("google/protobuf/io/strtod.h"),
+    Path("google/protobuf/io/zero_copy_stream.h"),
+    Path("google/protobuf/io/zero_copy_stream_impl.h"),
+    Path("google/protobuf/io/zero_copy_stream_impl_lite.h"),
+    Path("google/protobuf/map.h"),
+    Path("google/protobuf/map_entry_lite.h"),
+    Path("google/protobuf/map_field_lite.h"),
+    Path("google/protobuf/map_type_handler.h"),
+    Path("google/protobuf/message_lite.h"),
+    Path("google/protobuf/metadata_lite.h"),
+    Path("google/protobuf/parse_context.h"),
+    Path("google/protobuf/port.h"),
+    Path("google/protobuf/repeated_field.h"),
+    Path("google/protobuf/repeated_ptr_field.h"),
+    Path("google/protobuf/stubs/bytestream.h"),
+    Path("google/protobuf/stubs/callback.h"),
+    Path("google/protobuf/stubs/casts.h"),
+    Path("google/protobuf/stubs/common.h"),
+    Path("google/protobuf/stubs/hash.h"),
+    Path("google/protobuf/stubs/logging.h"),
+    Path("google/protobuf/stubs/macros.h"),
+    Path("google/protobuf/stubs/map_util.h"),
+    Path("google/protobuf/stubs/mutex.h"),
+    Path("google/protobuf/stubs/once.h"),
+    Path("google/protobuf/stubs/platform_macros.h"),
+    Path("google/protobuf/stubs/port.h"),
+    Path("google/protobuf/stubs/status.h"),
+    Path("google/protobuf/stubs/stl_util.h"),
+    Path("google/protobuf/stubs/stringpiece.h"),
+    Path("google/protobuf/stubs/strutil.h"),
+    Path("google/protobuf/stubs/template_util.h"),
+    Path("google/protobuf/wire_format_lite.h"),
+}
+protobuf_sources = {
+    Path("any.cc"),
+    Path("any.pb.cc"),
+    Path("api.pb.cc"),
+    Path("compiler/importer.cc"),
+    Path("compiler/parser.cc"),
+    Path("descriptor.cc"),
+    Path("descriptor.pb.cc"),
+    Path("descriptor_database.cc"),
+    Path("duration.pb.cc"),
+    Path("dynamic_message.cc"),
+    Path("empty.pb.cc"),
+    Path("extension_set_heavy.cc"),
+    Path("field_mask.pb.cc"),
+    Path("generated_message_bases.cc"),
+    Path("generated_message_reflection.cc"),
+    Path("generated_message_tctable_full.cc"),
+    Path("io/gzip_stream.cc"),
+    Path("io/printer.cc"),
+    Path("io/tokenizer.cc"),
+    Path("map_field.cc"),
+    Path("message.cc"),
+    Path("reflection_ops.cc"),
+    Path("service.cc"),
+    Path("source_context.pb.cc"),
+    Path("struct.pb.cc"),
+    Path("stubs/substitute.cc"),
+    Path("text_format.cc"),
+    Path("timestamp.pb.cc"),
+    Path("type.pb.cc"),
+    Path("unknown_field_set.cc"),
+    Path("util/delimited_message_util.cc"),
+    Path("util/field_comparator.cc"),
+    Path("util/field_mask_util.cc"),
+    Path("util/internal/datapiece.cc"),
+    Path("util/internal/default_value_objectwriter.cc"),
+    Path("util/internal/error_listener.cc"),
+    Path("util/internal/field_mask_utility.cc"),
+    Path("util/internal/json_escaping.cc"),
+    Path("util/internal/json_objectwriter.cc"),
+    Path("util/internal/json_stream_parser.cc"),
+    Path("util/internal/object_writer.cc"),
+    Path("util/internal/proto_writer.cc"),
+    Path("util/internal/protostream_objectsource.cc"),
+    Path("util/internal/protostream_objectwriter.cc"),
+    Path("util/internal/type_info.cc"),
+    Path("util/internal/utility.cc"),
+    Path("util/json_util.cc"),
+    Path("util/message_differencer.cc"),
+    Path("util/time_util.cc"),
+    Path("util/type_resolver_util.cc"),
+    Path("wire_format.cc"),
+    Path("wrappers.pb.cc"),
+}
 
-protobuf_sources = set(
-    [
-        "google/protobuf/any.cc",
-        "google/protobuf/any.pb.cc",
-        "google/protobuf/api.pb.cc",
-        "google/protobuf/compiler/importer.cc",
-        "google/protobuf/compiler/parser.cc",
-        "google/protobuf/descriptor.cc",
-        "google/protobuf/descriptor.pb.cc",
-        "google/protobuf/descriptor_database.cc",
-        "google/protobuf/duration.pb.cc",
-        "google/protobuf/dynamic_message.cc",
-        "google/protobuf/empty.pb.cc",
-        "google/protobuf/extension_set_heavy.cc",
-        "google/protobuf/field_mask.pb.cc",
-        "google/protobuf/generated_message_bases.cc",
-        "google/protobuf/generated_message_reflection.cc",
-        "google/protobuf/generated_message_tctable_full.cc",
-        "google/protobuf/io/gzip_stream.cc",
-        "google/protobuf/io/printer.cc",
-        "google/protobuf/io/tokenizer.cc",
-        "google/protobuf/map_field.cc",
-        "google/protobuf/message.cc",
-        "google/protobuf/reflection_ops.cc",
-        "google/protobuf/service.cc",
-        "google/protobuf/source_context.pb.cc",
-        "google/protobuf/struct.pb.cc",
-        "google/protobuf/stubs/substitute.cc",
-        "google/protobuf/text_format.cc",
-        "google/protobuf/timestamp.pb.cc",
-        "google/protobuf/type.pb.cc",
-        "google/protobuf/unknown_field_set.cc",
-        "google/protobuf/util/delimited_message_util.cc",
-        "google/protobuf/util/field_comparator.cc",
-        "google/protobuf/util/field_mask_util.cc",
-        "google/protobuf/util/internal/datapiece.cc",
-        "google/protobuf/util/internal/default_value_objectwriter.cc",
-        "google/protobuf/util/internal/error_listener.cc",
-        "google/protobuf/util/internal/field_mask_utility.cc",
-        "google/protobuf/util/internal/json_escaping.cc",
-        "google/protobuf/util/internal/json_objectwriter.cc",
-        "google/protobuf/util/internal/json_stream_parser.cc",
-        "google/protobuf/util/internal/object_writer.cc",
-        "google/protobuf/util/internal/proto_writer.cc",
-        "google/protobuf/util/internal/protostream_objectsource.cc",
-        "google/protobuf/util/internal/protostream_objectwriter.cc",
-        "google/protobuf/util/internal/type_info.cc",
-        "google/protobuf/util/internal/utility.cc",
-        "google/protobuf/util/json_util.cc",
-        "google/protobuf/util/message_differencer.cc",
-        "google/protobuf/util/time_util.cc",
-        "google/protobuf/util/type_resolver_util.cc",
-        "google/protobuf/wire_format.cc",
-        "google/protobuf/wrappers.pb.cc",
-    ]
-)
+protobuf_includes = {
+    Path("google/protobuf/any.pb.h"),
+    Path("google/protobuf/api.pb.h"),
+    Path("google/protobuf/compiler/importer.h"),
+    Path("google/protobuf/compiler/parser.h"),
+    Path("google/protobuf/descriptor.h"),
+    Path("google/protobuf/descriptor.pb.h"),
+    Path("google/protobuf/descriptor_database.h"),
+    Path("google/protobuf/duration.pb.h"),
+    Path("google/protobuf/dynamic_message.h"),
+    Path("google/protobuf/empty.pb.h"),
+    Path("google/protobuf/field_access_listener.h"),
+    Path("google/protobuf/field_mask.pb.h"),
+    Path("google/protobuf/generated_enum_reflection.h"),
+    Path("google/protobuf/generated_message_bases.h"),
+    Path("google/protobuf/generated_message_reflection.h"),
+    Path("google/protobuf/io/gzip_stream.h"),
+    Path("google/protobuf/io/printer.h"),
+    Path("google/protobuf/io/tokenizer.h"),
+    Path("google/protobuf/map_entry.h"),
+    Path("google/protobuf/map_field.h"),
+    Path("google/protobuf/map_field_inl.h"),
+    Path("google/protobuf/message.h"),
+    Path("google/protobuf/metadata.h"),
+    Path("google/protobuf/reflection.h"),
+    Path("google/protobuf/reflection_internal.h"),
+    Path("google/protobuf/reflection_ops.h"),
+    Path("google/protobuf/service.h"),
+    Path("google/protobuf/source_context.pb.h"),
+    Path("google/protobuf/struct.pb.h"),
+    Path("google/protobuf/text_format.h"),
+    Path("google/protobuf/timestamp.pb.h"),
+    Path("google/protobuf/type.pb.h"),
+    Path("google/protobuf/unknown_field_set.h"),
+    Path("google/protobuf/util/delimited_message_util.h"),
+    Path("google/protobuf/util/field_comparator.h"),
+    Path("google/protobuf/util/field_mask_util.h"),
+    Path("google/protobuf/util/json_util.h"),
+    Path("google/protobuf/util/message_differencer.h"),
+    Path("google/protobuf/util/time_util.h"),
+    Path("google/protobuf/util/type_resolver.h"),
+    Path("google/protobuf/util/type_resolver_util.h"),
+    Path("google/protobuf/wire_format.h"),
+    Path("google/protobuf/wrappers.pb.h"),
+}
 
-protobuf_includes = set(
-    [
-        "google/protobuf/any.pb.h",
-        "google/protobuf/api.pb.h",
-        "google/protobuf/compiler/importer.h",
-        "google/protobuf/compiler/parser.h",
-        "google/protobuf/descriptor.h",
-        "google/protobuf/descriptor.pb.h",
-        "google/protobuf/descriptor_database.h",
-        "google/protobuf/duration.pb.h",
-        "google/protobuf/dynamic_message.h",
-        "google/protobuf/empty.pb.h",
-        "google/protobuf/field_access_listener.h",
-        "google/protobuf/field_mask.pb.h",
-        "google/protobuf/generated_enum_reflection.h",
-        "google/protobuf/generated_message_bases.h",
-        "google/protobuf/generated_message_reflection.h",
-        "google/protobuf/io/gzip_stream.h",
-        "google/protobuf/io/printer.h",
-        "google/protobuf/io/tokenizer.h",
-        "google/protobuf/map_entry.h",
-        "google/protobuf/map_field.h",
-        "google/protobuf/map_field_inl.h",
-        "google/protobuf/message.h",
-        "google/protobuf/metadata.h",
-        "google/protobuf/reflection.h",
-        "google/protobuf/reflection_internal.h",
-        "google/protobuf/reflection_ops.h",
-        "google/protobuf/service.h",
-        "google/protobuf/source_context.pb.h",
-        "google/protobuf/struct.pb.h",
-        "google/protobuf/text_format.h",
-        "google/protobuf/timestamp.pb.h",
-        "google/protobuf/type.pb.h",
-        "google/protobuf/unknown_field_set.h",
-        "google/protobuf/util/delimited_message_util.h",
-        "google/protobuf/util/field_comparator.h",
-        "google/protobuf/util/field_mask_util.h",
-        "google/protobuf/util/json_util.h",
-        "google/protobuf/util/message_differencer.h",
-        "google/protobuf/util/time_util.h",
-        "google/protobuf/util/type_resolver.h",
-        "google/protobuf/util/type_resolver_util.h",
-        "google/protobuf/wire_format.h",
-        "google/protobuf/wrappers.pb.h",
-    ]
-)
-
-protobuf_internal_includes = set(
-    [
-        "google/protobuf/port_def.inc",
-        "google/protobuf/port_undef.inc",
-        "google/protobuf/stubs/int128.h",
-        "google/protobuf/stubs/mathutil.h",
-        "google/protobuf/stubs/statusor.h",
-        "google/protobuf/stubs/status_macros.h",
-        "google/protobuf/stubs/stringprintf.h",
-        "google/protobuf/stubs/substitute.h",
-        "google/protobuf/stubs/time.h",
-        "google/protobuf/util/internal/constants.h",
-        "google/protobuf/util/internal/datapiece.h",
-        "google/protobuf/util/internal/default_value_objectwriter.h",
-        "google/protobuf/util/internal/error_listener.h",
-        "google/protobuf/util/internal/field_mask_utility.h",
-        "google/protobuf/util/internal/json_escaping.h",
-        "google/protobuf/util/internal/json_objectwriter.h",
-        "google/protobuf/util/internal/json_stream_parser.h",
-        "google/protobuf/util/internal/location_tracker.h",
-        "google/protobuf/util/internal/object_location_tracker.h",
-        "google/protobuf/util/internal/object_source.h",
-        "google/protobuf/util/internal/object_writer.h",
-        "google/protobuf/util/internal/proto_writer.h",
-        "google/protobuf/util/internal/protostream_objectsource.h",
-        "google/protobuf/util/internal/protostream_objectwriter.h",
-        "google/protobuf/util/internal/structured_objectwriter.h",
-        "google/protobuf/util/internal/type_info.h",
-        "google/protobuf/util/internal/utility.h",
-    ]
-)
+protobuf_internal_includes = {
+    Path("google/protobuf/port_def.inc"),
+    Path("google/protobuf/port_undef.inc"),
+    Path("google/protobuf/stubs/int128.h"),
+    Path("google/protobuf/stubs/mathutil.h"),
+    Path("google/protobuf/stubs/statusor.h"),
+    Path("google/protobuf/stubs/status_macros.h"),
+    Path("google/protobuf/stubs/stringprintf.h"),
+    Path("google/protobuf/stubs/substitute.h"),
+    Path("google/protobuf/stubs/time.h"),
+    Path("google/protobuf/util/internal/constants.h"),
+    Path("google/protobuf/util/internal/datapiece.h"),
+    Path("google/protobuf/util/internal/default_value_objectwriter.h"),
+    Path("google/protobuf/util/internal/error_listener.h"),
+    Path("google/protobuf/util/internal/field_mask_utility.h"),
+    Path("google/protobuf/util/internal/json_escaping.h"),
+    Path("google/protobuf/util/internal/json_objectwriter.h"),
+    Path("google/protobuf/util/internal/json_stream_parser.h"),
+    Path("google/protobuf/util/internal/location_tracker.h"),
+    Path("google/protobuf/util/internal/object_location_tracker.h"),
+    Path("google/protobuf/util/internal/object_source.h"),
+    Path("google/protobuf/util/internal/object_writer.h"),
+    Path("google/protobuf/util/internal/proto_writer.h"),
+    Path("google/protobuf/util/internal/protostream_objectsource.h"),
+    Path("google/protobuf/util/internal/protostream_objectwriter.h"),
+    Path("google/protobuf/util/internal/structured_objectwriter.h"),
+    Path("google/protobuf/util/internal/type_info.h"),
+    Path("google/protobuf/util/internal/utility.h"),
+}
 
 use_src_files = protobuf_lite_sources | protobuf_sources
 use_include_files = (
@@ -240,38 +230,29 @@ use_include_files = (
 )
 
 
-def matches(dp, f, files):
-    if not dp.startswith(os.path.join(".", "src")):
-        return False
-    p = dp[6:] + "/" + f
-    return p in files
-
-
-def copy_upstream_src(wpilib_root):
-    upstream_root = os.path.abspath(".")
-    wpiutil = os.path.join(wpilib_root, "wpiutil")
+def copy_upstream_src(wpilib_root: Path):
+    upstream_root = Path(".").absolute()
+    wpiutil = wpilib_root / "wpiutil"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/protobuf/src",
         "src/main/native/thirdparty/protobuf/include",
     ]:
-        shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True)
+        shutil.rmtree(wpiutil / d, ignore_errors=True)
 
     # Copy protobuf source files into allwpilib
-    src_files = walk_if(".", lambda dp, f: matches(dp, f, use_src_files))
-    src_files = [f[22:] for f in src_files]
-    os.chdir(os.path.join(upstream_root, "src/google/protobuf"))
-    copy_to(src_files, os.path.join(wpiutil, "src/main/native/thirdparty/protobuf/src"))
+    os.chdir(upstream_root / "src/google/protobuf")
+    walk_cwd_and_copy_if(
+        lambda dp, f: dp / f in use_src_files,
+        wpiutil / "src/main/native/thirdparty/protobuf/src",
+    )
 
     # Copy protobuf header files into allwpilib
-    os.chdir(upstream_root)
-    include_files = walk_if(".", lambda dp, f: matches(dp, f, use_include_files))
-    include_files = [f[6:] for f in include_files]
-    os.chdir(os.path.join(upstream_root, "src"))
-    copy_to(
-        include_files,
-        os.path.join(wpiutil, "src/main/native/thirdparty/protobuf/include"),
+    os.chdir(upstream_root / "src")
+    walk_cwd_and_copy_if(
+        lambda dp, f: dp / f in use_include_files,
+        wpiutil / "src/main/native/thirdparty/protobuf/include",
     )
 
 

upstream_utils/sleipnir.py

@@ -1,55 +1,59 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
-from upstream_utils import Lib, copy_to
+from upstream_utils import Lib, has_prefix, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    wpimath = os.path.join(wpilib_root, "wpimath")
+def copy_upstream_src(wpilib_root: Path):
+    wpimath = wpilib_root / "wpimath"
 
     # Delete old install
     for d in [
         "src/main/native/thirdparty/sleipnir/src",
         "src/main/native/thirdparty/sleipnir/include",
     ]:
-        shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True)
+        shutil.rmtree(wpimath / d, ignore_errors=True)
 
-    # Copy Sleipnir source files into allwpilib
-    src_files = [os.path.join(dp, f) for dp, dn, fn in os.walk("src") for f in fn]
-    src_files = copy_to(
-        src_files, os.path.join(wpimath, "src/main/native/thirdparty/sleipnir")
+    # Copy Sleipnir files into allwpilib
+    walk_cwd_and_copy_if(
+        lambda dp, f: (has_prefix(dp, Path("include")) or has_prefix(dp, Path("src")))
+        or f == ".clang-format"
+        or f == ".clang-tidy"
+        or f == ".styleguide"
+        or f == ".styleguide-license",
+        wpimath / "src/main/native/thirdparty/sleipnir",
     )
 
-    # Copy Sleipnir header files into allwpilib
-    include_files = [
-        os.path.join(dp, f)
-        for dp, dn, fn in os.walk("include")
-        for f in fn
-        if not f.endswith("small_vector.hpp")
-    ]
-    include_files = copy_to(
-        include_files, os.path.join(wpimath, "src/main/native/thirdparty/sleipnir")
-    )
+    # Write shim for wpi::SmallVector
+    (wpimath / "src/main/native/thirdparty/sleipnir/include/gch").mkdir()
+    with open(
+        wpimath / "src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp",
+        "w",
+    ) as f:
+        f.write(
+            """// Copyright (c) Sleipnir contributors
 
-    for filename in [
-        ".clang-format",
-        ".clang-tidy",
-        ".styleguide",
-        ".styleguide-license",
-    ]:
-        shutil.copyfile(
-            filename,
-            os.path.join(wpimath, "src/main/native/thirdparty/sleipnir", filename),
+#pragma once
+
+#include <wpi/SmallVector.h>
+
+namespace gch {
+
+template <typename T>
+using small_vector = wpi::SmallVector<T>;
+
+}  // namespace gch
+"""
         )
 
 
 def main():
     name = "sleipnir"
     url = "https://github.com/SleipnirGroup/Sleipnir"
-    # main on 2024-12-07
-    tag = "01206ab17d741f4c45a7faeb56b8a5442df1681c"
+    # main on 2025-05-18
+    tag = "2cc18ff6d25ee0a9bd0f9993a0a41a61a28bda3e"
 
     sleipnir = Lib(name, url, tag, copy_upstream_src)
     sleipnir.main()

upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch

@@ -1,30 +1,32 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Wed, 29 May 2024 16:29:55 -0700
-Subject: [PATCH 1/3] Use fmtlib
+Subject: [PATCH 1/8] Use fmtlib
 
 ---
  include/.styleguide             |  1 +
- include/sleipnir/util/Print.hpp | 31 ++++++++++++++++++-------------
- 2 files changed, 19 insertions(+), 13 deletions(-)
+ include/sleipnir/util/print.hpp | 31 ++++++++++++++++++-------------
+ src/optimization/problem.cpp    |  2 +-
+ 3 files changed, 20 insertions(+), 14 deletions(-)
 
 diff --git a/include/.styleguide b/include/.styleguide
-index 8fb61fdf9cc5ceff633d3126f0579eef25a1326f..6a7f8ed28f9cb037c9746a7e0ef5e110481d9825 100644
+index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
 --- a/include/.styleguide
 +++ b/include/.styleguide
-@@ -12,4 +12,5 @@ licenseUpdateExclude {
+@@ -8,5 +8,6 @@ cppSrcFileInclude {
  
  includeOtherLibs {
    ^Eigen/
 +  ^fmt/
+   ^gch/
  }
-diff --git a/include/sleipnir/util/Print.hpp b/include/sleipnir/util/Print.hpp
-index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0984626fa 100644
---- a/include/sleipnir/util/Print.hpp
-+++ b/include/sleipnir/util/Print.hpp
-@@ -3,52 +3,57 @@
- #pragma once
+diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp
+index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644
+--- a/include/sleipnir/util/print.hpp
++++ b/include/sleipnir/util/print.hpp
+@@ -4,10 +4,15 @@
  
+ #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
  #include <cstdio>
 -#include <print>
  #include <system_error>
@@ -36,7 +38,11 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0
 +#include <fmt/core.h>
 +#endif
 +
- namespace sleipnir {
+ #endif
+ 
+ namespace slp {
+@@ -15,45 +20,45 @@ namespace slp {
+ #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
  
  /**
 - * Wrapper around std::print() that squelches write failure exceptions.
@@ -93,3 +99,16 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0
    } catch (const std::system_error&) {
    }
  }
+diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
+index 31115490867146ec166604bcc61731d7891a9f22..81863808d329a53d4162ce0624a3b8e8afc32dfc 100644
+--- a/src/optimization/problem.cpp
++++ b/src/optimization/problem.cpp
+@@ -335,7 +335,7 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) {
+     slp::println("  ↳ executed {} iterations", options.max_iterations);
+   }
+   if (std::isfinite(options.timeout.count())) {
+-    slp::println("  ↳ {} elapsed", options.timeout);
++    slp::println("  ↳ {} elapsed", options.timeout.count());
+   }
+ }
+ 

upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch

@@ -1,610 +1,77 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <calcmogul@gmail.com>
 Date: Sun, 16 Jun 2024 12:08:49 -0700
-Subject: [PATCH 2/3] Use wpi::SmallVector
+Subject: [PATCH 2/8] Use wpi::SmallVector
 
 ---
- include/.styleguide                            |  1 +
- include/sleipnir/autodiff/Expression.hpp       | 13 +++++++------
- include/sleipnir/autodiff/ExpressionGraph.hpp  | 15 ++++++++-------
- include/sleipnir/autodiff/Hessian.hpp          |  4 ++--
- include/sleipnir/autodiff/Jacobian.hpp         | 10 +++++-----
- include/sleipnir/autodiff/Variable.hpp         | 10 +++++-----
- include/sleipnir/autodiff/VariableMatrix.hpp   |  4 ++--
- include/sleipnir/optimization/Multistart.hpp   |  7 ++++---
- .../optimization/OptimizationProblem.hpp       |  8 ++++----
- include/sleipnir/util/Pool.hpp                 |  7 ++++---
- include/sleipnir/util/Spy.hpp                  |  4 ++--
- src/.styleguide                                |  1 +
- src/optimization/solver/InteriorPoint.cpp      |  4 ++--
- src/optimization/solver/SQP.cpp                |  4 ++--
- .../solver/util/FeasibilityRestoration.hpp     | 18 +++++++++---------
- src/optimization/solver/util/Filter.hpp        |  4 ++--
- 16 files changed, 60 insertions(+), 54 deletions(-)
+ include/sleipnir/autodiff/expression.hpp      | 4 ++--
+ include/sleipnir/autodiff/variable.hpp        | 5 ++---
+ include/sleipnir/autodiff/variable_matrix.hpp | 4 ++--
+ 3 files changed, 6 insertions(+), 7 deletions(-)
 
-diff --git a/include/.styleguide b/include/.styleguide
-index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644
---- a/include/.styleguide
-+++ b/include/.styleguide
-@@ -13,4 +13,5 @@ licenseUpdateExclude {
- includeOtherLibs {
-   ^Eigen/
-   ^fmt/
-+  ^wpi/
- }
-diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp
-index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b359cc8752 100644
---- a/include/sleipnir/autodiff/Expression.hpp
-+++ b/include/sleipnir/autodiff/Expression.hpp
-@@ -11,11 +11,12 @@
- #include <numbers>
- #include <utility>
+diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
+index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd527825ba90adb 100644
+--- a/include/sleipnir/autodiff/expression.hpp
++++ b/include/sleipnir/autodiff/expression.hpp
+@@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true;
+ struct Expression;
  
-+#include <wpi/SmallVector.h>
-+
- #include "sleipnir/autodiff/ExpressionType.hpp"
- #include "sleipnir/util/IntrusiveSharedPtr.hpp"
- #include "sleipnir/util/Pool.hpp"
- #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir::detail {
- 
-@@ -29,8 +30,8 @@ inline constexpr bool kUsePoolAllocator = true;
- 
- struct SLEIPNIR_DLLEXPORT Expression;
- 
--inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr);
--inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr);
-+inline void IntrusiveSharedPtrIncRefCount(Expression* expr);
-+inline void IntrusiveSharedPtrDecRefCount(Expression* expr);
+ inline constexpr void inc_ref_count(Expression* expr);
+-inline constexpr void dec_ref_count(Expression* expr);
++inline void dec_ref_count(Expression* expr);
  
  /**
   * Typedef for intrusive shared pointer to Expression.
-@@ -418,7 +419,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x);
+@@ -680,7 +680,7 @@ inline constexpr void inc_ref_count(Expression* expr) {
   *
   * @param expr The shared pointer's managed object.
   */
--inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) {
-+inline void IntrusiveSharedPtrIncRefCount(Expression* expr) {
-   ++expr->refCount;
- }
- 
-@@ -427,12 +428,12 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) {
-  *
-  * @param expr The shared pointer's managed object.
-  */
--inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) {
-+inline void IntrusiveSharedPtrDecRefCount(Expression* expr) {
+-inline constexpr void dec_ref_count(Expression* expr) {
++inline void dec_ref_count(Expression* expr) {
    // If a deeply nested tree is being deallocated all at once, calling the
    // Expression destructor when expr's refcount reaches zero can cause a stack
    // overflow. Instead, we iterate over its children to decrement their
-   // refcounts and deallocate them.
--  small_vector<Expression*> stack;
-+  wpi::SmallVector<Expression*> stack;
-   stack.emplace_back(expr);
- 
-   while (!stack.empty()) {
-diff --git a/include/sleipnir/autodiff/ExpressionGraph.hpp b/include/sleipnir/autodiff/ExpressionGraph.hpp
-index c614195d82ad022dfd0c3f6f8240b042c0056c2f..714bcbb82907e754138347334c7fca8a7ccf055d 100644
---- a/include/sleipnir/autodiff/ExpressionGraph.hpp
-+++ b/include/sleipnir/autodiff/ExpressionGraph.hpp
-@@ -4,10 +4,11 @@
- 
- #include <span>
- 
-+#include <wpi/SmallVector.h>
-+
- #include "sleipnir/autodiff/Expression.hpp"
- #include "sleipnir/util/FunctionRef.hpp"
- #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir::detail {
- 
-@@ -36,7 +37,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
-     // https://en.wikipedia.org/wiki/Breadth-first_search
- 
-     // BFS list sorted from parent to child.
--    small_vector<Expression*> stack;
-+    wpi::SmallVector<Expression*> stack;
- 
-     stack.emplace_back(root.Get());
- 
-@@ -119,7 +120,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
-    *
-    * @param wrt Variables with respect to which to compute the gradient.
+diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
+index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d57645032 100644
+--- a/include/sleipnir/autodiff/variable.hpp
++++ b/include/sleipnir/autodiff/variable.hpp
+@@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable {
+   /**
+    * Constructs an empty Variable.
     */
--  small_vector<ExpressionPtr> GenerateGradientTree(
-+  wpi::SmallVector<ExpressionPtr> GenerateGradientTree(
-       std::span<const ExpressionPtr> wrt) const {
-     // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
-     // for background on reverse accumulation automatic differentiation.
-@@ -128,7 +129,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
-       wrt[row]->row = row;
-     }
- 
--    small_vector<ExpressionPtr> grad;
-+    wpi::SmallVector<ExpressionPtr> grad;
-     grad.reserve(wrt.size());
-     for (size_t row = 0; row < wrt.size(); ++row) {
-       grad.emplace_back(MakeExpressionPtr());
-@@ -231,13 +232,13 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
- 
-  private:
-   // List that maps nodes to their respective row.
--  small_vector<int> m_rowList;
-+  wpi::SmallVector<int> m_rowList;
- 
-   // List for updating adjoints
--  small_vector<Expression*> m_adjointList;
-+  wpi::SmallVector<Expression*> m_adjointList;
- 
-   // List for updating values
--  small_vector<Expression*> m_valueList;
-+  wpi::SmallVector<Expression*> m_valueList;
- };
- 
- }  // namespace sleipnir::detail
-diff --git a/include/sleipnir/autodiff/Hessian.hpp b/include/sleipnir/autodiff/Hessian.hpp
-index 4563aa234bd7b0ec22e12d2fc0b6f4569bee7f39..2e60d89e95280bdac422b0d7dab955ba111b0059 100644
---- a/include/sleipnir/autodiff/Hessian.hpp
-+++ b/include/sleipnir/autodiff/Hessian.hpp
-@@ -6,6 +6,7 @@
- 
- #include <Eigen/Core>
- #include <Eigen/SparseCore>
-+#include <wpi/SmallVector.h>
- 
- #include "sleipnir/autodiff/ExpressionGraph.hpp"
- #include "sleipnir/autodiff/Jacobian.hpp"
-@@ -13,7 +14,6 @@
- #include "sleipnir/autodiff/Variable.hpp"
- #include "sleipnir/autodiff/VariableMatrix.hpp"
- #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -36,7 +36,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
-   Hessian(Variable variable, const VariableMatrix& wrt) noexcept
-       : m_jacobian{
-             [&] {
--              small_vector<detail::ExpressionPtr> wrtVec;
-+              wpi::SmallVector<detail::ExpressionPtr> wrtVec;
-               wrtVec.reserve(wrt.size());
-               for (auto& elem : wrt) {
-                 wrtVec.emplace_back(elem.expr);
-diff --git a/include/sleipnir/autodiff/Jacobian.hpp b/include/sleipnir/autodiff/Jacobian.hpp
-index ac00c11ef8c947cbe95c58081bdbfb42bf901051..0c660156c80f94539383b8f0d01d7853e41e0297 100644
---- a/include/sleipnir/autodiff/Jacobian.hpp
-+++ b/include/sleipnir/autodiff/Jacobian.hpp
-@@ -5,13 +5,13 @@
- #include <utility>
- 
- #include <Eigen/SparseCore>
-+#include <wpi/SmallVector.h>
- 
- #include "sleipnir/autodiff/ExpressionGraph.hpp"
- #include "sleipnir/autodiff/Profiler.hpp"
- #include "sleipnir/autodiff/Variable.hpp"
- #include "sleipnir/autodiff/VariableMatrix.hpp"
- #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -81,7 +81,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
-   VariableMatrix Get() const {
-     VariableMatrix result{m_variables.Rows(), m_wrt.Rows()};
- 
--    small_vector<detail::ExpressionPtr> wrtVec;
-+    wpi::SmallVector<detail::ExpressionPtr> wrtVec;
-     wrtVec.reserve(m_wrt.size());
-     for (auto& elem : m_wrt) {
-       wrtVec.emplace_back(elem.expr);
-@@ -138,16 +138,16 @@ class SLEIPNIR_DLLEXPORT Jacobian {
-   VariableMatrix m_variables;
-   VariableMatrix m_wrt;
- 
--  small_vector<detail::ExpressionGraph> m_graphs;
-+  wpi::SmallVector<detail::ExpressionGraph> m_graphs;
- 
-   Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
- 
-   // Cached triplets for gradients of linear rows
--  small_vector<Eigen::Triplet<double>> m_cachedTriplets;
-+  wpi::SmallVector<Eigen::Triplet<double>> m_cachedTriplets;
- 
-   // List of row indices for nonlinear rows whose graients will be computed in
-   // Value()
--  small_vector<int> m_nonlinearRows;
-+  wpi::SmallVector<int> m_nonlinearRows;
- 
-   Profiler m_profiler;
- };
-diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp
-index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5e84ca34e 100644
---- a/include/sleipnir/autodiff/Variable.hpp
-+++ b/include/sleipnir/autodiff/Variable.hpp
-@@ -10,6 +10,7 @@
- #include <vector>
- 
- #include <Eigen/Core>
-+#include <wpi/SmallVector.h>
- 
- #include "sleipnir/autodiff/Expression.hpp"
- #include "sleipnir/autodiff/ExpressionGraph.hpp"
-@@ -17,7 +18,6 @@
- #include "sleipnir/util/Concepts.hpp"
- #include "sleipnir/util/Print.hpp"
- #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -445,8 +445,8 @@ template <typename LHS, typename RHS>
-           (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
-           (!std::same_as<std::decay_t<LHS>, double> ||
-            !std::same_as<std::decay_t<RHS>, double>)
--small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
--  small_vector<Variable> constraints;
-+wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
-+  wpi::SmallVector<Variable> constraints;
- 
-   if constexpr (ScalarLike<std::decay_t<LHS>> &&
-                 ScalarLike<std::decay_t<RHS>>) {
-@@ -534,7 +534,7 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
-  */
- struct SLEIPNIR_DLLEXPORT EqualityConstraints {
-   /// A vector of scalar equality constraints.
--  small_vector<Variable> constraints;
-+  wpi::SmallVector<Variable> constraints;
+-  explicit constexpr Variable(std::nullptr_t) : expr{nullptr} {}
++  explicit Variable(std::nullptr_t) : expr{nullptr} {}
  
    /**
-    * Concatenates multiple equality constraints.
-@@ -596,7 +596,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
-  */
- struct SLEIPNIR_DLLEXPORT InequalityConstraints {
-   /// A vector of scalar inequality constraints.
--  small_vector<Variable> constraints;
-+  wpi::SmallVector<Variable> constraints;
+    * Constructs a Variable from a floating point type.
+@@ -77,8 +77,7 @@ class SLEIPNIR_DLLEXPORT Variable {
+    *
+    * @param expr The autodiff variable.
+    */
+-  explicit constexpr Variable(detail::ExpressionPtr&& expr)
+-      : expr{std::move(expr)} {}
++  explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {}
  
    /**
-    * Concatenates multiple inequality constraints.
-diff --git a/include/sleipnir/autodiff/VariableMatrix.hpp b/include/sleipnir/autodiff/VariableMatrix.hpp
-index 47452b8988b3a1a96a78d28644200b1c4cdc89c9..57b09913d15e9590873ad7bf62e2baff9fbc5df9 100644
---- a/include/sleipnir/autodiff/VariableMatrix.hpp
-+++ b/include/sleipnir/autodiff/VariableMatrix.hpp
-@@ -11,6 +11,7 @@
- #include <vector>
- 
- #include <Eigen/Core>
-+#include <wpi/SmallVector.h>
- 
- #include "sleipnir/autodiff/Slice.hpp"
- #include "sleipnir/autodiff/Variable.hpp"
-@@ -18,7 +19,6 @@
- #include "sleipnir/util/Assert.hpp"
- #include "sleipnir/util/FunctionRef.hpp"
- #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -946,7 +946,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
-   }
- 
-  private:
--  small_vector<Variable> m_storage;
-+  wpi::SmallVector<Variable> m_storage;
-   int m_rows = 0;
-   int m_cols = 0;
- };
-diff --git a/include/sleipnir/optimization/Multistart.hpp b/include/sleipnir/optimization/Multistart.hpp
-index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d80c68cec 100644
---- a/include/sleipnir/optimization/Multistart.hpp
-+++ b/include/sleipnir/optimization/Multistart.hpp
-@@ -6,9 +6,10 @@
- #include <future>
- #include <span>
- 
-+#include <wpi/SmallVector.h>
-+
- #include "sleipnir/optimization/SolverStatus.hpp"
- #include "sleipnir/util/FunctionRef.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -44,14 +45,14 @@ MultistartResult<DecisionVariables> Multistart(
-         const DecisionVariables& initialGuess)>
-         solve,
-     std::span<const DecisionVariables> initialGuesses) {
--  small_vector<std::future<MultistartResult<DecisionVariables>>> futures;
-+  wpi::SmallVector<std::future<MultistartResult<DecisionVariables>>> futures;
-   futures.reserve(initialGuesses.size());
- 
-   for (const auto& initialGuess : initialGuesses) {
-     futures.emplace_back(std::async(std::launch::async, solve, initialGuess));
-   }
- 
--  small_vector<MultistartResult<DecisionVariables>> results;
-+  wpi::SmallVector<MultistartResult<DecisionVariables>> results;
-   results.reserve(futures.size());
- 
-   for (auto& future : futures) {
-diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp
-index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce6e047928 100644
---- a/include/sleipnir/optimization/OptimizationProblem.hpp
-+++ b/include/sleipnir/optimization/OptimizationProblem.hpp
-@@ -11,6 +11,7 @@
- #include <utility>
- 
- #include <Eigen/Core>
-+#include <wpi/SmallVector.h>
- 
- #include "sleipnir/autodiff/Variable.hpp"
- #include "sleipnir/autodiff/VariableMatrix.hpp"
-@@ -22,7 +23,6 @@
- #include "sleipnir/optimization/solver/SQP.hpp"
- #include "sleipnir/util/Print.hpp"
- #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -364,16 +364,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
-  private:
-   // The list of decision variables, which are the root of the problem's
-   // expression tree
--  small_vector<Variable> m_decisionVariables;
-+  wpi::SmallVector<Variable> m_decisionVariables;
- 
-   // The cost function: f(x)
-   std::optional<Variable> m_f;
- 
-   // The list of equality constraints: cₑ(x) = 0
--  small_vector<Variable> m_equalityConstraints;
-+  wpi::SmallVector<Variable> m_equalityConstraints;
- 
-   // The list of inequality constraints: cᵢ(x) ≥ 0
--  small_vector<Variable> m_inequalityConstraints;
-+  wpi::SmallVector<Variable> m_inequalityConstraints;
- 
-   // The user callback
-   std::function<bool(const SolverIterationInfo& info)> m_callback =
-diff --git a/include/sleipnir/util/Pool.hpp b/include/sleipnir/util/Pool.hpp
-index 441fa701d4972bc14973c6269d56d4a124deaee5..1951bd1ee8f3bee8d4a3c044c99354b0fd10031d 100644
---- a/include/sleipnir/util/Pool.hpp
-+++ b/include/sleipnir/util/Pool.hpp
-@@ -5,8 +5,9 @@
- #include <cstddef>
- #include <memory>
- 
-+#include <wpi/SmallVector.h>
-+
- #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -78,8 +79,8 @@ class SLEIPNIR_DLLEXPORT PoolResource {
-   }
- 
-  private:
--  small_vector<std::unique_ptr<std::byte[]>> m_buffer;
--  small_vector<void*> m_freeList;
-+  wpi::SmallVector<std::unique_ptr<std::byte[]>> m_buffer;
-+  wpi::SmallVector<void*> m_freeList;
-   size_t blocksPerChunk;
+    * Assignment operator for double.
+diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
+index 410f12873cfdf5d0d484653c6c3dac74ed96348a..1c6f9e8dade8bebce7aec18bbb9b5491acb1d977 100644
+--- a/include/sleipnir/autodiff/variable_matrix.hpp
++++ b/include/sleipnir/autodiff/variable_matrix.hpp
+@@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+    *
+    * @return Begin iterator.
+    */
+-  const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; }
++  const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
  
    /**
-diff --git a/include/sleipnir/util/Spy.hpp b/include/sleipnir/util/Spy.hpp
-index cb9b4e191545e96c2bade5f8f99b0bec376b656b..7f526a2d9968e76b385a0ddfb2edf5bab7274fb0 100644
---- a/include/sleipnir/util/Spy.hpp
-+++ b/include/sleipnir/util/Spy.hpp
-@@ -7,9 +7,9 @@
- #include <string_view>
+    * Returns end iterator.
+    *
+    * @return End iterator.
+    */
+-  const_iterator cend() const { return const_iterator{m_storage.cend()}; }
++  const_iterator cend() const { return const_iterator{m_storage.end()}; }
  
- #include <Eigen/SparseCore>
-+#include <wpi/SmallVector.h>
- 
- #include "sleipnir/util/SymbolExports.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -32,7 +32,7 @@ SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file,
-   const int cells_width = mat.cols() + 1;
-   const int cells_height = mat.rows();
- 
--  small_vector<uint8_t> cells;
-+  wpi::SmallVector<uint8_t> cells;
- 
-   // Allocate space for matrix of characters plus trailing newlines
-   cells.reserve(cells_width * cells_height);
-diff --git a/src/.styleguide b/src/.styleguide
-index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92503484c1 100644
---- a/src/.styleguide
-+++ b/src/.styleguide
-@@ -8,4 +8,5 @@ cppSrcFileInclude {
- 
- includeOtherLibs {
-   ^Eigen/
-+  ^wpi/
- }
-diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp
-index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e1429c475 100644
---- a/src/optimization/solver/InteriorPoint.cpp
-+++ b/src/optimization/solver/InteriorPoint.cpp
-@@ -9,6 +9,7 @@
- #include <limits>
- 
- #include <Eigen/SparseCholesky>
-+#include <wpi/SmallVector.h>
- 
- #include "optimization/RegularizedLDLT.hpp"
- #include "optimization/solver/util/ErrorEstimate.hpp"
-@@ -23,7 +24,6 @@
- #include "sleipnir/optimization/SolverExitCondition.hpp"
- #include "sleipnir/util/Print.hpp"
- #include "sleipnir/util/Spy.hpp"
--#include "sleipnir/util/small_vector.hpp"
- #include "util/ScopeExit.hpp"
- #include "util/ToMilliseconds.hpp"
- 
-@@ -226,7 +226,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
-   };
- 
-   // Kept outside the loop so its storage can be reused
--  small_vector<Eigen::Triplet<double>> triplets;
-+  wpi::SmallVector<Eigen::Triplet<double>> triplets;
- 
-   RegularizedLDLT solver;
- 
-diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp
-index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce78549a3f6 100644
---- a/src/optimization/solver/SQP.cpp
-+++ b/src/optimization/solver/SQP.cpp
-@@ -9,6 +9,7 @@
- #include <limits>
- 
- #include <Eigen/SparseCholesky>
-+#include <wpi/SmallVector.h>
- 
- #include "optimization/RegularizedLDLT.hpp"
- #include "optimization/solver/util/ErrorEstimate.hpp"
-@@ -22,7 +23,6 @@
- #include "sleipnir/optimization/SolverExitCondition.hpp"
- #include "sleipnir/util/Print.hpp"
- #include "sleipnir/util/Spy.hpp"
--#include "sleipnir/util/small_vector.hpp"
- #include "util/ScopeExit.hpp"
- #include "util/ToMilliseconds.hpp"
- 
-@@ -155,7 +155,7 @@ void SQP(std::span<Variable> decisionVariables,
-   Filter filter{f};
- 
-   // Kept outside the loop so its storage can be reused
--  small_vector<Eigen::Triplet<double>> triplets;
-+  wpi::SmallVector<Eigen::Triplet<double>> triplets;
- 
-   RegularizedLDLT solver;
- 
-diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp
-index feefe137adf0832b094a36d61201b15962138ded..79b5d99ae27de6049ba098888a965291e6b677fa 100644
---- a/src/optimization/solver/util/FeasibilityRestoration.hpp
-+++ b/src/optimization/solver/util/FeasibilityRestoration.hpp
-@@ -8,6 +8,7 @@
- #include <span>
- 
- #include <Eigen/Core>
-+#include <wpi/SmallVector.h>
- 
- #include "sleipnir/autodiff/Variable.hpp"
- #include "sleipnir/autodiff/VariableMatrix.hpp"
-@@ -16,7 +17,6 @@
- #include "sleipnir/optimization/SolverStatus.hpp"
- #include "sleipnir/optimization/solver/InteriorPoint.hpp"
- #include "sleipnir/util/FunctionRef.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- namespace sleipnir {
- 
-@@ -57,7 +57,7 @@ inline void FeasibilityRestoration(
-   constexpr double ρ = 1000.0;
-   double μ = config.tolerance / 10.0;
- 
--  small_vector<Variable> fr_decisionVariables;
-+  wpi::SmallVector<Variable> fr_decisionVariables;
-   fr_decisionVariables.reserve(decisionVariables.size() +
-                                2 * equalityConstraints.size());
- 
-@@ -70,7 +70,7 @@ inline void FeasibilityRestoration(
-     fr_decisionVariables.emplace_back();
-   }
- 
--  auto it = fr_decisionVariables.cbegin();
-+  auto it = fr_decisionVariables.begin();
- 
-   VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
-   it += decisionVariables.size();
-@@ -128,7 +128,7 @@ inline void FeasibilityRestoration(
-   }
- 
-   // cₑ(x) - pₑ + nₑ = 0
--  small_vector<Variable> fr_equalityConstraints;
-+  wpi::SmallVector<Variable> fr_equalityConstraints;
-   fr_equalityConstraints.assign(equalityConstraints.begin(),
-                                 equalityConstraints.end());
-   for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
-@@ -137,7 +137,7 @@ inline void FeasibilityRestoration(
-   }
- 
-   // cᵢ(x) - s - pᵢ + nᵢ = 0
--  small_vector<Variable> fr_inequalityConstraints;
-+  wpi::SmallVector<Variable> fr_inequalityConstraints;
- 
-   // pₑ ≥ 0
-   std::copy(p_e.begin(), p_e.end(),
-@@ -227,7 +227,7 @@ inline void FeasibilityRestoration(
- 
-   constexpr double ρ = 1000.0;
- 
--  small_vector<Variable> fr_decisionVariables;
-+  wpi::SmallVector<Variable> fr_decisionVariables;
-   fr_decisionVariables.reserve(decisionVariables.size() +
-                                2 * equalityConstraints.size() +
-                                2 * inequalityConstraints.size());
-@@ -243,7 +243,7 @@ inline void FeasibilityRestoration(
-     fr_decisionVariables.emplace_back();
-   }
- 
--  auto it = fr_decisionVariables.cbegin();
-+  auto it = fr_decisionVariables.begin();
- 
-   VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
-   it += decisionVariables.size();
-@@ -319,7 +319,7 @@ inline void FeasibilityRestoration(
-   }
- 
-   // cₑ(x) - pₑ + nₑ = 0
--  small_vector<Variable> fr_equalityConstraints;
-+  wpi::SmallVector<Variable> fr_equalityConstraints;
-   fr_equalityConstraints.assign(equalityConstraints.begin(),
-                                 equalityConstraints.end());
-   for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
-@@ -328,7 +328,7 @@ inline void FeasibilityRestoration(
-   }
- 
-   // cᵢ(x) - s - pᵢ + nᵢ = 0
--  small_vector<Variable> fr_inequalityConstraints;
-+  wpi::SmallVector<Variable> fr_inequalityConstraints;
-   fr_inequalityConstraints.assign(inequalityConstraints.begin(),
-                                   inequalityConstraints.end());
-   for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) {
-diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp
-index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54dbf64ec68 100644
---- a/src/optimization/solver/util/Filter.hpp
-+++ b/src/optimization/solver/util/Filter.hpp
-@@ -8,9 +8,9 @@
- #include <utility>
- 
- #include <Eigen/Core>
-+#include <wpi/SmallVector.h>
- 
- #include "sleipnir/autodiff/Variable.hpp"
--#include "sleipnir/util/small_vector.hpp"
- 
- // See docs/algorithms.md#Works_cited for citation definitions.
- 
-@@ -177,7 +177,7 @@ class Filter {
- 
-  private:
-   Variable* m_f = nullptr;
--  small_vector<FilterEntry> m_filter;
-+  wpi::SmallVector<FilterEntry> m_filter;
- };
- 
- }  // namespace sleipnir
+   /**
+    * Returns number of elements in matrix.

upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch

@@ -1,31 +0,0 @@
-From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
-From: Tyler Veness <calcmogul@gmail.com>
-Date: Wed, 26 Jun 2024 12:13:33 -0700
-Subject: [PATCH 3/3] Suppress clang-tidy false positives
-
----
- include/sleipnir/autodiff/Variable.hpp | 4 ++--
- 1 file changed, 2 insertions(+), 2 deletions(-)
-
-diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp
-index d192fb96e7984b7c0ca30262668c41e5e84ca34e..f25c6d153310a01700ee2390ecf35ffa8af7df11 100644
---- a/include/sleipnir/autodiff/Variable.hpp
-+++ b/include/sleipnir/autodiff/Variable.hpp
-@@ -541,7 +541,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
-    *
-    * @param equalityConstraints The list of EqualityConstraints to concatenate.
-    */
--  EqualityConstraints(
-+  EqualityConstraints(  // NOLINT
-       std::initializer_list<EqualityConstraints> equalityConstraints) {
-     for (const auto& elem : equalityConstraints) {
-       constraints.insert(constraints.end(), elem.constraints.begin(),
-@@ -604,7 +604,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
-    * @param inequalityConstraints The list of InequalityConstraints to
-    * concatenate.
-    */
--  InequalityConstraints(
-+  InequalityConstraints(  // NOLINT
-       std::initializer_list<InequalityConstraints> inequalityConstraints) {
-     for (const auto& elem : inequalityConstraints) {
-       constraints.insert(constraints.end(), elem.constraints.begin(),

upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch

@@ -0,0 +1,41 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Tue, 28 Jan 2025 22:19:14 -0800
+Subject: [PATCH 3/8] Use wpi::byteswap()
+
+---
+ include/.styleguide           | 1 +
+ include/sleipnir/util/spy.hpp | 3 ++-
+ 2 files changed, 3 insertions(+), 1 deletion(-)
+
+diff --git a/include/.styleguide b/include/.styleguide
+index 4f4c76204071f90bf49eddb8c2aceb583b5e09ba..03938557c2600a7a1f72c6b93c935602f5acb2b2 100644
+--- a/include/.styleguide
++++ b/include/.styleguide
+@@ -10,4 +10,5 @@ includeOtherLibs {
+   ^Eigen/
+   ^fmt/
+   ^gch/
++  ^wpi/
+ }
+diff --git a/include/sleipnir/util/spy.hpp b/include/sleipnir/util/spy.hpp
+index a2f94803e3744cee771669210d1af883160e9896..8cd7d4353aad20153af5cd7a818fa55889d35721 100644
+--- a/include/sleipnir/util/spy.hpp
++++ b/include/sleipnir/util/spy.hpp
+@@ -12,6 +12,7 @@
+ #include <string_view>
+ 
+ #include <Eigen/SparseCore>
++#include <wpi/bit.h>
+ 
+ #include "sleipnir/util/symbol_exports.hpp"
+ 
+@@ -115,7 +116,7 @@ class SLEIPNIR_DLLEXPORT Spy {
+    */
+   void write32le(int32_t num) {
+     if constexpr (std::endian::native != std::endian::little) {
+-      num = std::byteswap(num);
++      num = wpi::byteswap(num);
+     }
+     m_file.write(reinterpret_cast<char*>(&num), sizeof(num));
+   }

upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch

@@ -0,0 +1,63 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Tue, 28 Jan 2025 22:19:31 -0800
+Subject: [PATCH 4/8] Replace std::to_underlying()
+
+---
+ src/optimization/problem.cpp   | 9 ++++-----
+ src/util/print_diagnostics.hpp | 6 +++---
+ 2 files changed, 7 insertions(+), 8 deletions(-)
+
+diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
+index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff87ac8eea 100644
+--- a/src/optimization/problem.cpp
++++ b/src/optimization/problem.cpp
+@@ -7,7 +7,6 @@
+ #include <memory>
+ #include <optional>
+ #include <ranges>
+-#include <utility>
+ 
+ #include <Eigen/Core>
+ #include <Eigen/SparseCore>
+@@ -346,11 +345,11 @@ void Problem::print_problem_analysis() {
+   // Print problem structure
+   slp::println("\nProblem structure:");
+   slp::println("  ↳ {} cost function",
+-               types[std::to_underlying(cost_function_type())]);
++               types[static_cast<uint8_t>(cost_function_type())]);
+   slp::println("  ↳ {} equality constraints",
+-               types[std::to_underlying(equality_constraint_type())]);
++               types[static_cast<uint8_t>(equality_constraint_type())]);
+   slp::println("  ↳ {} inequality constraints",
+-               types[std::to_underlying(inequality_constraint_type())]);
++               types[static_cast<uint8_t>(inequality_constraint_type())]);
+ 
+   if (m_decision_variables.size() == 1) {
+     slp::print("\n1 decision variable\n");
+@@ -362,7 +361,7 @@ void Problem::print_problem_analysis() {
+       [](const gch::small_vector<Variable>& constraints) {
+         std::array<size_t, 5> counts{};
+         for (const auto& constraint : constraints) {
+-          ++counts[std::to_underlying(constraint.type())];
++          ++counts[static_cast<uint8_t>(constraint.type())];
+         }
+         for (const auto& [count, name] :
+              std::views::zip(counts, std::array{"empty", "constant", "linear",
+diff --git a/src/util/print_diagnostics.hpp b/src/util/print_diagnostics.hpp
+index fde36957c0258f6e3cd435ef6224d60407012ff7..82e0e082b0e40153dcb2fcd2c655a412a8a9540a 100644
+--- a/src/util/print_diagnostics.hpp
++++ b/src/util/print_diagnostics.hpp
+@@ -238,9 +238,9 @@ void print_iteration_diagnostics(int iterations, IterationType type,
+   slp::println(
+       "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
+       "{:.2e} {:2d}│",
+-      iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error,
+-      cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
+-      backtracks);
++      iterations, ITERATION_TYPES[static_cast<uint8_t>(type)], to_ms(time),
++      error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α,
++      dual_α, backtracks);
+ }
+ #else
+ #define print_iteration_diagnostics(...)

upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch

@@ -0,0 +1,53 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Sat, 8 Feb 2025 13:42:36 -0800
+Subject: [PATCH 5/8] Replace std::views::zip()
+
+---
+ include/sleipnir/autodiff/adjoint_expression_graph.hpp | 5 ++++-
+ src/optimization/problem.cpp                           | 9 +++++----
+ 2 files changed, 9 insertions(+), 5 deletions(-)
+
+diff --git a/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
+index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d8252a0db 100644
+--- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp
++++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
+@@ -155,7 +155,10 @@ class AdjointExpressionGraph {
+         }
+       }
+     } else {
+-      for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) {
++      for (size_t i = 0; i < m_top_list.size(); ++i) {
++        const auto& col = m_col_list[i];
++        const auto& node = m_top_list[i];
++
+         // Append adjoints of wrt to sparse matrix triplets
+         if (col != -1 && node->adjoint != 0.0) {
+           triplets.emplace_back(row, col, node->adjoint);
+diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
+index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba0f36bc78 100644
+--- a/src/optimization/problem.cpp
++++ b/src/optimization/problem.cpp
+@@ -6,7 +6,6 @@
+ #include <cmath>
+ #include <memory>
+ #include <optional>
+-#include <ranges>
+ 
+ #include <Eigen/Core>
+ #include <Eigen/SparseCore>
+@@ -363,9 +362,11 @@ void Problem::print_problem_analysis() {
+         for (const auto& constraint : constraints) {
+           ++counts[static_cast<uint8_t>(constraint.type())];
+         }
+-        for (const auto& [count, name] :
+-             std::views::zip(counts, std::array{"empty", "constant", "linear",
+-                                                "quadratic", "nonlinear"})) {
++        for (size_t i = 0; i < counts.size(); ++i) {
++          constexpr std::array names{"empty", "constant", "linear", "quadratic",
++                                     "nonlinear"};
++          const auto& count = counts[i];
++          const auto& name = names[i];
+           if (count > 0) {
+             slp::println("  ↳ {} {}", count, name);
+           }

upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch

@@ -0,0 +1,22 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Mon, 10 Feb 2025 11:37:02 -0800
+Subject: [PATCH 6/8] Suppress clang-tidy false positives
+
+---
+ include/sleipnir/autodiff/variable.hpp | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
+index 9f79a82763213dc712cce4c2a322289d57645032..17e7eb7cc2c7c7599eaba97d8ec80972524c1599 100644
+--- a/include/sleipnir/autodiff/variable.hpp
++++ b/include/sleipnir/autodiff/variable.hpp
+@@ -626,7 +626,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
+    * @param inequality_constraints The list of InequalityConstraints to
+    * concatenate.
+    */
+-  InequalityConstraints(
++  InequalityConstraints(  // NOLINT
+       std::initializer_list<InequalityConstraints> inequality_constraints) {
+     for (const auto& elem : inequality_constraints) {
+       constraints.insert(constraints.end(), elem.constraints.begin(),

upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch

@@ -0,0 +1,34 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Mon, 24 Feb 2025 15:12:03 -0800
+Subject: [PATCH 7/8] Suppress GCC 12 warning false positive
+
+---
+ include/sleipnir/autodiff/variable_matrix.hpp | 7 +++++++
+ 1 file changed, 7 insertions(+)
+
+diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
+index 1c6f9e8dade8bebce7aec18bbb9b5491acb1d977..dee43f926d304e1f4900bd57b99cd613e808f58e 100644
+--- a/include/sleipnir/autodiff/variable_matrix.hpp
++++ b/include/sleipnir/autodiff/variable_matrix.hpp
+@@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     VariableMatrix result(VariableMatrix::empty, lhs.rows(), rhs.cols());
+ 
++#if __GNUC__ >= 12
++#pragma GCC diagnostic push
++#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
++#endif
+     for (int i = 0; i < lhs.rows(); ++i) {
+       for (int j = 0; j < rhs.cols(); ++j) {
+         Variable sum;
+@@ -590,6 +594,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+         result[i, j] = sum;
+       }
+     }
++#if __GNUC__ >= 12
++#pragma GCC diagnostic pop
++#endif
+ 
+     return result;
+   }

upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch

@@ -0,0 +1,915 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: Tyler Veness <calcmogul@gmail.com>
+Date: Sat, 12 Apr 2025 16:28:47 -0700
+Subject: [PATCH 8/8] Revert "Use multidimensional array subscript operator
+ (#843)"
+
+This reverts commit f9b2c450bbbf6f14b194b8b81708d032a6431ee0.
+---
+ include/sleipnir/autodiff/hessian.hpp         |   4 +-
+ include/sleipnir/autodiff/jacobian.hpp        |   4 +-
+ include/sleipnir/autodiff/variable.hpp        |  26 +----
+ include/sleipnir/autodiff/variable_block.hpp  |  70 +++++------
+ include/sleipnir/autodiff/variable_matrix.hpp | 110 ++++++------------
+ include/sleipnir/control/ocp.hpp              |  14 +--
+ include/sleipnir/optimization/problem.hpp     |   6 +-
+ src/autodiff/variable_matrix.cpp              |  66 +++++------
+ 8 files changed, 118 insertions(+), 182 deletions(-)
+
+diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp
+index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67bef1b441 100644
+--- a/include/sleipnir/autodiff/hessian.hpp
++++ b/include/sleipnir/autodiff/hessian.hpp
+@@ -103,9 +103,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
+       auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
+       for (int col = 0; col < m_wrt.rows(); ++col) {
+         if (grad[col].expr != nullptr) {
+-          result[row, col] = std::move(grad[col]);
++          result(row, col) = std::move(grad[col]);
+         } else {
+-          result[row, col] = Variable{0.0};
++          result(row, col) = Variable{0.0};
+         }
+       }
+     }
+diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
+index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a719b7e5b5 100644
+--- a/include/sleipnir/autodiff/jacobian.hpp
++++ b/include/sleipnir/autodiff/jacobian.hpp
+@@ -95,9 +95,9 @@ class SLEIPNIR_DLLEXPORT Jacobian {
+       auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
+       for (int col = 0; col < m_wrt.rows(); ++col) {
+         if (grad[col].expr != nullptr) {
+-          result[row, col] = std::move(grad[col]);
++          result(row, col) = std::move(grad[col]);
+         } else {
+-          result[row, col] = Variable{0.0};
++          result(row, col) = Variable{0.0};
+         }
+       }
+     }
+diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
+index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da23034457a 100644
+--- a/include/sleipnir/autodiff/variable.hpp
++++ b/include/sleipnir/autodiff/variable.hpp
+@@ -505,11 +505,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
+     for (int row = 0; row < rhs.rows(); ++row) {
+       for (int col = 0; col < rhs.cols(); ++col) {
+         // Make right-hand side zero
+-        if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
+-          constraints.emplace_back(lhs - rhs(row, col));
+-        } else {
+-          constraints.emplace_back(lhs - rhs[row, col]);
+-        }
++        constraints.emplace_back(lhs - rhs(row, col));
+       }
+     }
+   } else if constexpr (MatrixLike<LHS> && ScalarLike<RHS>) {
+@@ -518,11 +514,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
+     for (int row = 0; row < lhs.rows(); ++row) {
+       for (int col = 0; col < lhs.cols(); ++col) {
+         // Make right-hand side zero
+-        if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
+-          constraints.emplace_back(lhs(row, col) - rhs);
+-        } else {
+-          constraints.emplace_back(lhs[row, col] - rhs);
+-        }
++        constraints.emplace_back(lhs(row, col) - rhs);
+       }
+     }
+   } else if constexpr (MatrixLike<LHS> && MatrixLike<RHS>) {
+@@ -532,19 +524,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
+     for (int row = 0; row < lhs.rows(); ++row) {
+       for (int col = 0; col < lhs.cols(); ++col) {
+         // Make right-hand side zero
+-        if constexpr (EigenMatrixLike<std::decay_t<LHS>> &&
+-                      EigenMatrixLike<std::decay_t<RHS>>) {
+-          constraints.emplace_back(lhs(row, col) - rhs(row, col));
+-        } else if constexpr (EigenMatrixLike<std::decay_t<LHS>> &&
+-                             SleipnirMatrixLike<std::decay_t<RHS>>) {
+-          constraints.emplace_back(lhs(row, col) - rhs[row, col]);
+-        } else if constexpr (SleipnirMatrixLike<std::decay_t<LHS>> &&
+-                             EigenMatrixLike<std::decay_t<RHS>>) {
+-          constraints.emplace_back(lhs[row, col] - rhs(row, col));
+-        } else if constexpr (SleipnirMatrixLike<std::decay_t<LHS>> &&
+-                             SleipnirMatrixLike<std::decay_t<RHS>>) {
+-          constraints.emplace_back(lhs[row, col] - rhs[row, col]);
+-        }
++        constraints.emplace_back(lhs(row, col) - rhs(row, col));
+       }
+     }
+   }
+diff --git a/include/sleipnir/autodiff/variable_block.hpp b/include/sleipnir/autodiff/variable_block.hpp
+index f1c1ca0dc3fde663c3e74f6fca4b89b119cf377d..632d44beb5b3dae29b9829c52a6168fee39fe537 100644
+--- a/include/sleipnir/autodiff/variable_block.hpp
++++ b/include/sleipnir/autodiff/variable_block.hpp
+@@ -50,7 +50,7 @@ class VariableBlock {
+ 
+       for (int row = 0; row < rows(); ++row) {
+         for (int col = 0; col < cols(); ++col) {
+-          (*this)[row, col] = values[row, col];
++          (*this)(row, col) = values(row, col);
+         }
+       }
+     }
+@@ -85,7 +85,7 @@ class VariableBlock {
+ 
+       for (int row = 0; row < rows(); ++row) {
+         for (int col = 0; col < cols(); ++col) {
+-          (*this)[row, col] = values[row, col];
++          (*this)(row, col) = values(row, col);
+         }
+       }
+     }
+@@ -152,7 +152,7 @@ class VariableBlock {
+   VariableBlock<Mat>& operator=(ScalarLike auto value) {
+     slp_assert(rows() == 1 && cols() == 1);
+ 
+-    (*this)[0, 0] = value;
++    (*this)(0, 0) = value;
+ 
+     return *this;
+   }
+@@ -167,7 +167,7 @@ class VariableBlock {
+   void set_value(double value) {
+     slp_assert(rows() == 1 && cols() == 1);
+ 
+-    (*this)[0, 0].set_value(value);
++    (*this)(0, 0).set_value(value);
+   }
+ 
+   /**
+@@ -182,7 +182,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] = values(row, col);
++        (*this)(row, col) = values(row, col);
+       }
+     }
+ 
+@@ -201,7 +201,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col].set_value(values(row, col));
++        (*this)(row, col).set_value(values(row, col));
+       }
+     }
+   }
+@@ -217,7 +217,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] = values[row, col];
++        (*this)(row, col) = values(row, col);
+       }
+     }
+     return *this;
+@@ -234,7 +234,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] = std::move(values[row, col]);
++        (*this)(row, col) = std::move(values(row, col));
+       }
+     }
+     return *this;
+@@ -247,13 +247,13 @@ class VariableBlock {
+    * @param col The scalar subblock's column.
+    * @return A scalar subblock at the given row and column.
+    */
+-  Variable& operator[](int row, int col)
++  Variable& operator()(int row, int col)
+     requires(!std::is_const_v<Mat>)
+   {
+     slp_assert(row >= 0 && row < rows());
+     slp_assert(col >= 0 && col < cols());
+-    return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
+-                    m_col_slice.start + col * m_col_slice.step];
++    return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
++                    m_col_slice.start + col * m_col_slice.step);
+   }
+ 
+   /**
+@@ -263,11 +263,11 @@ class VariableBlock {
+    * @param col The scalar subblock's column.
+    * @return A scalar subblock at the given row and column.
+    */
+-  const Variable& operator[](int row, int col) const {
++  const Variable& operator()(int row, int col) const {
+     slp_assert(row >= 0 && row < rows());
+     slp_assert(col >= 0 && col < cols());
+-    return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
+-                    m_col_slice.start + col * m_col_slice.step];
++    return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
++                    m_col_slice.start + col * m_col_slice.step);
+   }
+ 
+   /**
+@@ -280,7 +280,7 @@ class VariableBlock {
+     requires(!std::is_const_v<Mat>)
+   {
+     slp_assert(row >= 0 && row < rows() * cols());
+-    return (*this)[row / cols(), row % cols()];
++    return (*this)(row / cols(), row % cols());
+   }
+ 
+   /**
+@@ -291,7 +291,7 @@ class VariableBlock {
+    */
+   const Variable& operator[](int row) const {
+     slp_assert(row >= 0 && row < rows() * cols());
+-    return (*this)[row / cols(), row % cols()];
++    return (*this)(row / cols(), row % cols());
+   }
+ 
+   /**
+@@ -309,8 +309,8 @@ class VariableBlock {
+     slp_assert(col_offset >= 0 && col_offset <= cols());
+     slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
+     slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
+-    return (*this)[Slice{row_offset, row_offset + block_rows, 1},
+-                   Slice{col_offset, col_offset + block_cols, 1}];
++    return (*this)({row_offset, row_offset + block_rows, 1},
++                   {col_offset, col_offset + block_cols, 1});
+   }
+ 
+   /**
+@@ -328,8 +328,8 @@ class VariableBlock {
+     slp_assert(col_offset >= 0 && col_offset <= cols());
+     slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
+     slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
+-    return (*this)[Slice{row_offset, row_offset + block_rows, 1},
+-                   Slice{col_offset, col_offset + block_cols, 1}];
++    return (*this)({row_offset, row_offset + block_rows, 1},
++                   {col_offset, col_offset + block_cols, 1});
+   }
+ 
+   /**
+@@ -339,7 +339,7 @@ class VariableBlock {
+    * @param col_slice The column slice.
+    * @return A slice of the variable matrix.
+    */
+-  VariableBlock<Mat> operator[](Slice row_slice, Slice col_slice) {
++  VariableBlock<Mat> operator()(Slice row_slice, Slice col_slice) {
+     int row_slice_length = row_slice.adjust(m_row_slice_length);
+     int col_slice_length = col_slice.adjust(m_col_slice_length);
+     return VariableBlock{
+@@ -359,7 +359,7 @@ class VariableBlock {
+    * @param col_slice The column slice.
+    * @return A slice of the variable matrix.
+    */
+-  const VariableBlock<const Mat> operator[](Slice row_slice,
++  const VariableBlock<const Mat> operator()(Slice row_slice,
+                                             Slice col_slice) const {
+     int row_slice_length = row_slice.adjust(m_row_slice_length);
+     int col_slice_length = col_slice.adjust(m_col_slice_length);
+@@ -385,7 +385,7 @@ class VariableBlock {
+    * @param col_slice_length The column slice length.
+    * @return A slice of the variable matrix.
+    */
+-  VariableBlock<Mat> operator[](Slice row_slice, int row_slice_length,
++  VariableBlock<Mat> operator()(Slice row_slice, int row_slice_length,
+                                 Slice col_slice, int col_slice_length) {
+     return VariableBlock{
+         *m_mat,
+@@ -409,7 +409,7 @@ class VariableBlock {
+    * @param col_slice_length The column slice length.
+    * @return A slice of the variable matrix.
+    */
+-  const VariableBlock<const Mat> operator[](Slice row_slice,
++  const VariableBlock<const Mat> operator()(Slice row_slice,
+                                             int row_slice_length,
+                                             Slice col_slice,
+                                             int col_slice_length) const {
+@@ -524,7 +524,7 @@ class VariableBlock {
+   VariableBlock<Mat>& operator*=(const ScalarLike auto& rhs) {
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] *= rhs;
++        (*this)(row, col) *= rhs;
+       }
+     }
+ 
+@@ -542,7 +542,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] /= rhs[0, 0];
++        (*this)(row, col) /= rhs(0, 0);
+       }
+     }
+ 
+@@ -558,7 +558,7 @@ class VariableBlock {
+   VariableBlock<Mat>& operator/=(const ScalarLike auto& rhs) {
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] /= rhs;
++        (*this)(row, col) /= rhs;
+       }
+     }
+ 
+@@ -576,7 +576,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] += rhs[row, col];
++        (*this)(row, col) += rhs(row, col);
+       }
+     }
+ 
+@@ -594,7 +594,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] += rhs;
++        (*this)(row, col) += rhs;
+       }
+     }
+ 
+@@ -612,7 +612,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] -= rhs[row, col];
++        (*this)(row, col) -= rhs(row, col);
+       }
+     }
+ 
+@@ -630,7 +630,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] -= rhs;
++        (*this)(row, col) -= rhs;
+       }
+     }
+ 
+@@ -655,7 +655,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        result[col, row] = (*this)[row, col];
++        result(col, row) = (*this)(row, col);
+       }
+     }
+ 
+@@ -686,8 +686,8 @@ class VariableBlock {
+   double value(int row, int col) {
+     slp_assert(row >= 0 && row < rows());
+     slp_assert(col >= 0 && col < cols());
+-    return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
+-                    m_col_slice.start + col * m_col_slice.step]
++    return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
++                    m_col_slice.start + col * m_col_slice.step)
+         .value();
+   }
+ 
+@@ -731,7 +731,7 @@ class VariableBlock {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        result[row, col] = unary_op((*this)[row, col]);
++        result(row, col) = unary_op((*this)(row, col));
+       }
+     }
+ 
+diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
+index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be3562186991c7a 100644
+--- a/include/sleipnir/autodiff/variable_matrix.hpp
++++ b/include/sleipnir/autodiff/variable_matrix.hpp
+@@ -211,7 +211,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < values.rows(); ++row) {
+       for (int col = 0; col < values.cols(); ++col) {
+-        (*this)[row, col] = values(row, col);
++        (*this)(row, col) = values(row, col);
+       }
+     }
+ 
+@@ -229,7 +229,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+   VariableMatrix& operator=(ScalarLike auto value) {
+     slp_assert(rows() == 1 && cols() == 1);
+ 
+-    (*this)[0, 0] = value;
++    (*this)(0, 0) = value;
+ 
+     return *this;
+   }
+@@ -246,7 +246,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < values.rows(); ++row) {
+       for (int col = 0; col < values.cols(); ++col) {
+-        (*this)[row, col].set_value(values(row, col));
++        (*this)(row, col).set_value(values(row, col));
+       }
+     }
+   }
+@@ -280,7 +280,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+     m_storage.reserve(rows() * cols());
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        m_storage.emplace_back(values[row, col]);
++        m_storage.emplace_back(values(row, col));
+       }
+     }
+   }
+@@ -295,7 +295,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+     m_storage.reserve(rows() * cols());
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        m_storage.emplace_back(values[row, col]);
++        m_storage.emplace_back(values(row, col));
+       }
+     }
+   }
+@@ -340,7 +340,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+    * @param col The block column.
+    * @return A block pointing to the given row and column.
+    */
+-  Variable& operator[](int row, int col) {
++  Variable& operator()(int row, int col) {
+     slp_assert(row >= 0 && row < rows());
+     slp_assert(col >= 0 && col < cols());
+     return m_storage[row * cols() + col];
+@@ -353,7 +353,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+    * @param col The block column.
+    * @return A block pointing to the given row and column.
+    */
+-  const Variable& operator[](int row, int col) const {
++  const Variable& operator()(int row, int col) const {
+     slp_assert(row >= 0 && row < rows());
+     slp_assert(col >= 0 && col < cols());
+     return m_storage[row * cols() + col];
+@@ -426,7 +426,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+    * @param col_slice The column slice.
+    * @return A slice of the variable matrix.
+    */
+-  VariableBlock<VariableMatrix> operator[](Slice row_slice, Slice col_slice) {
++  VariableBlock<VariableMatrix> operator()(Slice row_slice, Slice col_slice) {
+     int row_slice_length = row_slice.adjust(rows());
+     int col_slice_length = col_slice.adjust(cols());
+     return VariableBlock{*this, std::move(row_slice), row_slice_length,
+@@ -440,7 +440,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+    * @param col_slice The column slice.
+    * @return A slice of the variable matrix.
+    */
+-  const VariableBlock<const VariableMatrix> operator[](Slice row_slice,
++  const VariableBlock<const VariableMatrix> operator()(Slice row_slice,
+                                                        Slice col_slice) const {
+     int row_slice_length = row_slice.adjust(rows());
+     int col_slice_length = col_slice.adjust(cols());
+@@ -461,7 +461,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+    * @return A slice of the variable matrix.
+    *
+    */
+-  VariableBlock<VariableMatrix> operator[](Slice row_slice,
++  VariableBlock<VariableMatrix> operator()(Slice row_slice,
+                                            int row_slice_length,
+                                            Slice col_slice,
+                                            int col_slice_length) {
+@@ -481,7 +481,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+    * @param col_slice_length The column slice length.
+    * @return A slice of the variable matrix.
+    */
+-  const VariableBlock<const VariableMatrix> operator[](
++  const VariableBlock<const VariableMatrix> operator()(
+       Slice row_slice, int row_slice_length, Slice col_slice,
+       int col_slice_length) const {
+     return VariableBlock{*this, std::move(row_slice), row_slice_length,
+@@ -581,17 +581,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+       for (int j = 0; j < rhs.cols(); ++j) {
+         Variable sum;
+         for (int k = 0; k < lhs.cols(); ++k) {
+-          if constexpr (SleipnirMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
+-            sum += lhs[i, k] * rhs[k, j];
+-          } else if constexpr (SleipnirMatrixLike<LHS> &&
+-                               EigenMatrixLike<RHS>) {
+-            sum += lhs[i, k] * rhs(k, j);
+-          } else if constexpr (EigenMatrixLike<LHS> &&
+-                               SleipnirMatrixLike<RHS>) {
+-            sum += lhs(i, k) * rhs[k, j];
+-          }
++          sum += lhs(i, k) * rhs(k, j);
+         }
+-        result[i, j] = sum;
++        result(i, j) = sum;
+       }
+     }
+ #if __GNUC__ >= 12
+@@ -613,7 +605,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < result.rows(); ++row) {
+       for (int col = 0; col < result.cols(); ++col) {
+-        result[row, col] = lhs[row, col] * rhs;
++        result(row, col) = lhs(row, col) * rhs;
+       }
+     }
+ 
+@@ -632,11 +624,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < result.rows(); ++row) {
+       for (int col = 0; col < result.cols(); ++col) {
+-        if constexpr (SleipnirMatrixLike<decltype(lhs)>) {
+-          result[row, col] = lhs[row, col] * rhs;
+-        } else {
+-          result[row, col] = lhs(row, col) * rhs;
+-        }
++        result(row, col) = lhs(row, col) * rhs;
+       }
+     }
+ 
+@@ -655,7 +643,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < result.rows(); ++row) {
+       for (int col = 0; col < result.cols(); ++col) {
+-        result[row, col] = rhs[row, col] * lhs;
++        result(row, col) = rhs(row, col) * lhs;
+       }
+     }
+ 
+@@ -674,11 +662,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < result.rows(); ++row) {
+       for (int col = 0; col < result.cols(); ++col) {
+-        if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
+-          result[row, col] = rhs[row, col] * lhs;
+-        } else {
+-          result[row, col] = rhs(row, col) * lhs;
+-        }
++        result(row, col) = rhs(row, col) * lhs;
+       }
+     }
+ 
+@@ -698,13 +682,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+       for (int j = 0; j < rhs.cols(); ++j) {
+         Variable sum;
+         for (int k = 0; k < cols(); ++k) {
+-          if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
+-            sum += (*this)[i, k] * rhs[k, j];
+-          } else {
+-            sum += (*this)[i, k] * rhs(k, j);
+-          }
++          sum += (*this)(i, k) * rhs(k, j);
+         }
+-        (*this)[i, j] = sum;
++        (*this)(i, j) = sum;
+       }
+     }
+ 
+@@ -720,7 +700,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+   VariableMatrix& operator*=(const ScalarLike auto& rhs) {
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < rhs.cols(); ++col) {
+-        (*this)[row, col] *= rhs;
++        (*this)(row, col) *= rhs;
+       }
+     }
+ 
+@@ -740,11 +720,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < result.rows(); ++row) {
+       for (int col = 0; col < result.cols(); ++col) {
+-        if constexpr (SleipnirMatrixLike<decltype(lhs)>) {
+-          result[row, col] = lhs[row, col] / rhs;
+-        } else {
+-          result[row, col] = lhs(row, col) / rhs;
+-        }
++        result(row, col) = lhs(row, col) / rhs;
+       }
+     }
+ 
+@@ -760,7 +736,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+   VariableMatrix& operator/=(const ScalarLike auto& rhs) {
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] /= rhs;
++        (*this)(row, col) /= rhs;
+       }
+     }
+ 
+@@ -784,13 +760,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < result.rows(); ++row) {
+       for (int col = 0; col < result.cols(); ++col) {
+-        if constexpr (SleipnirMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
+-          result[row, col] = lhs[row, col] + rhs[row, col];
+-        } else if constexpr (SleipnirMatrixLike<LHS> && EigenMatrixLike<RHS>) {
+-          result[row, col] = lhs[row, col] + rhs(row, col);
+-        } else if constexpr (EigenMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
+-          result[row, col] = lhs(row, col) + rhs[row, col];
+-        }
++        result(row, col) = lhs(row, col) + rhs(row, col);
+       }
+     }
+ 
+@@ -808,11 +778,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
+-          (*this)[row, col] += rhs[row, col];
+-        } else {
+-          (*this)[row, col] += rhs(row, col);
+-        }
++        (*this)(row, col) += rhs(row, col);
+       }
+     }
+ 
+@@ -830,7 +796,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] += rhs;
++        (*this)(row, col) += rhs;
+       }
+     }
+ 
+@@ -854,13 +820,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < result.rows(); ++row) {
+       for (int col = 0; col < result.cols(); ++col) {
+-        if constexpr (SleipnirMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
+-          result[row, col] = lhs[row, col] - rhs[row, col];
+-        } else if constexpr (SleipnirMatrixLike<LHS> && EigenMatrixLike<RHS>) {
+-          result[row, col] = lhs[row, col] - rhs(row, col);
+-        } else if constexpr (EigenMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
+-          result[row, col] = lhs(row, col) - rhs[row, col];
+-        }
++        result(row, col) = lhs(row, col) - rhs(row, col);
+       }
+     }
+ 
+@@ -878,11 +838,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
+-          (*this)[row, col] -= rhs[row, col];
+-        } else {
+-          (*this)[row, col] -= rhs(row, col);
+-        }
++        (*this)(row, col) -= rhs(row, col);
+       }
+     }
+ 
+@@ -900,7 +856,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        (*this)[row, col] -= rhs;
++        (*this)(row, col) -= rhs;
+       }
+     }
+ 
+@@ -918,7 +874,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < result.rows(); ++row) {
+       for (int col = 0; col < result.cols(); ++col) {
+-        result[row, col] = -lhs[row, col];
++        result(row, col) = -lhs(row, col);
+       }
+     }
+ 
+@@ -930,7 +886,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+    */
+   operator Variable() const {  // NOLINT
+     slp_assert(rows() == 1 && cols() == 1);
+-    return (*this)[0, 0];
++    return (*this)(0, 0);
+   }
+ 
+   /**
+@@ -943,7 +899,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        result[col, row] = (*this)[row, col];
++        result(col, row) = (*this)(row, col);
+       }
+     }
+ 
+@@ -1017,7 +973,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+ 
+     for (int row = 0; row < rows(); ++row) {
+       for (int col = 0; col < cols(); ++col) {
+-        result[row, col] = unary_op((*this)[row, col]);
++        result(row, col) = unary_op((*this)(row, col));
+       }
+     }
+ 
+@@ -1199,7 +1155,7 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce(
+ 
+   for (int row = 0; row < lhs.rows(); ++row) {
+     for (int col = 0; col < lhs.cols(); ++col) {
+-      result[row, col] = binary_op(lhs[row, col], rhs[row, col]);
++      result(row, col) = binary_op(lhs(row, col), rhs(row, col));
+     }
+   }
+ 
+diff --git a/include/sleipnir/control/ocp.hpp b/include/sleipnir/control/ocp.hpp
+index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bcd3be3776 100644
+--- a/include/sleipnir/control/ocp.hpp
++++ b/include/sleipnir/control/ocp.hpp
+@@ -180,7 +180,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
+     if (m_timestep_method == TimestepMethod::FIXED) {
+       m_DT = VariableMatrix{1, m_num_steps + 1};
+       for (int i = 0; i < num_steps + 1; ++i) {
+-        m_DT[0, i] = m_dt.count();
++        m_DT(0, i) = m_dt.count();
+       }
+     } else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) {
+       Variable dt = decision_variable();
+@@ -189,12 +189,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
+       // Set the member variable matrix to track the decision variable
+       m_DT = VariableMatrix{1, m_num_steps + 1};
+       for (int i = 0; i < num_steps + 1; ++i) {
+-        m_DT[0, i] = dt;
++        m_DT(0, i) = dt;
+       }
+     } else if (m_timestep_method == TimestepMethod::VARIABLE) {
+       m_DT = decision_variable(1, m_num_steps + 1);
+       for (int i = 0; i < num_steps + 1; ++i) {
+-        m_DT[0, i].set_value(m_dt.count());
++        m_DT(0, i).set_value(m_dt.count());
+       }
+     }
+ 
+@@ -270,7 +270,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
+     for (int i = 0; i < m_num_steps + 1; ++i) {
+       auto x = X().col(i);
+       auto u = U().col(i);
+-      auto dt = this->dt()[0, i];
++      auto dt = this->dt()(0, i);
+       callback(time, x, u, dt);
+ 
+       time += dt;
+@@ -377,7 +377,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
+ 
+     // Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/
+     for (int i = 0; i < m_num_steps; ++i) {
+-      Variable h = dt()[0, i];
++      Variable h = dt()(0, i);
+ 
+       auto& f = m_dynamics_function;
+ 
+@@ -412,7 +412,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
+       auto x_begin = X().col(i);
+       auto x_end = X().col(i + 1);
+       auto u = U().col(i);
+-      Variable dt = this->dt()[0, i];
++      Variable dt = this->dt()(0, i);
+ 
+       if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
+         subject_to(x_end == rk4<const decltype(m_dynamics_function)&,
+@@ -433,7 +433,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
+       auto x_begin = X().col(i);
+       auto x_end = X().col(i + 1);
+       auto u = U().col(i);
+-      Variable dt = this->dt()[0, i];
++      Variable dt = this->dt()(0, i);
+ 
+       if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
+         x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix,
+diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
+index b7a868657c704487049efaf6b3972b1f7b72bfb4..b484ec08d6c50bf42fbaa1d5b4c66a20cb11a922 100644
+--- a/include/sleipnir/optimization/problem.hpp
++++ b/include/sleipnir/optimization/problem.hpp
+@@ -78,7 +78,7 @@ class SLEIPNIR_DLLEXPORT Problem {
+     for (int row = 0; row < rows; ++row) {
+       for (int col = 0; col < cols; ++col) {
+         m_decision_variables.emplace_back();
+-        vars[row, col] = m_decision_variables.back();
++        vars(row, col) = m_decision_variables.back();
+       }
+     }
+ 
+@@ -113,8 +113,8 @@ class SLEIPNIR_DLLEXPORT Problem {
+     for (int row = 0; row < rows; ++row) {
+       for (int col = 0; col <= row; ++col) {
+         m_decision_variables.emplace_back();
+-        vars[row, col] = m_decision_variables.back();
+-        vars[col, row] = m_decision_variables.back();
++        vars(row, col) = m_decision_variables.back();
++        vars(col, row) = m_decision_variables.back();
+       }
+     }
+ 
+diff --git a/src/autodiff/variable_matrix.cpp b/src/autodiff/variable_matrix.cpp
+index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac766fcad2a4 100644
+--- a/src/autodiff/variable_matrix.cpp
++++ b/src/autodiff/variable_matrix.cpp
+@@ -12,17 +12,17 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
+ 
+   if (A.rows() == 1 && A.cols() == 1) {
+     // Compute optimal inverse instead of using Eigen's general solver
+-    return B[0, 0] / A[0, 0];
++    return B(0, 0) / A(0, 0);
+   } else if (A.rows() == 2 && A.cols() == 2) {
+     // Compute optimal inverse instead of using Eigen's general solver
+     //
+     // [a  b]⁻¹  ___1___ [ d  −b]
+     // [c  d]  = ad − bc [−c   a]
+ 
+-    const auto& a = A[0, 0];
+-    const auto& b = A[0, 1];
+-    const auto& c = A[1, 0];
+-    const auto& d = A[1, 1];
++    const auto& a = A(0, 0);
++    const auto& b = A(0, 1);
++    const auto& c = A(1, 0);
++    const auto& d = A(1, 1);
+ 
+     slp::VariableMatrix adj_A{{d, -b}, {-c, a}};
+     auto det_A = a * d - b * c;
+@@ -39,15 +39,15 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
+     //
+     // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D
+ 
+-    const auto& a = A[0, 0];
+-    const auto& b = A[0, 1];
+-    const auto& c = A[0, 2];
+-    const auto& d = A[1, 0];
+-    const auto& e = A[1, 1];
+-    const auto& f = A[1, 2];
+-    const auto& g = A[2, 0];
+-    const auto& h = A[2, 1];
+-    const auto& i = A[2, 2];
++    const auto& a = A(0, 0);
++    const auto& b = A(0, 1);
++    const auto& c = A(0, 2);
++    const auto& d = A(1, 0);
++    const auto& e = A(1, 1);
++    const auto& f = A(1, 2);
++    const auto& g = A(2, 0);
++    const auto& h = A(2, 1);
++    const auto& i = A(2, 2);
+ 
+     auto ae = a * e;
+     auto af = a * f;
+@@ -87,22 +87,22 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
+     //
+     // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D
+ 
+-    const auto& a = A[0, 0];
+-    const auto& b = A[0, 1];
+-    const auto& c = A[0, 2];
+-    const auto& d = A[0, 3];
+-    const auto& e = A[1, 0];
+-    const auto& f = A[1, 1];
+-    const auto& g = A[1, 2];
+-    const auto& h = A[1, 3];
+-    const auto& i = A[2, 0];
+-    const auto& j = A[2, 1];
+-    const auto& k = A[2, 2];
+-    const auto& l = A[2, 3];
+-    const auto& m = A[3, 0];
+-    const auto& n = A[3, 1];
+-    const auto& o = A[3, 2];
+-    const auto& p = A[3, 3];
++    const auto& a = A(0, 0);
++    const auto& b = A(0, 1);
++    const auto& c = A(0, 2);
++    const auto& d = A(0, 3);
++    const auto& e = A(1, 0);
++    const auto& f = A(1, 1);
++    const auto& g = A(1, 2);
++    const auto& h = A(1, 3);
++    const auto& i = A(2, 0);
++    const auto& j = A(2, 1);
++    const auto& k = A(2, 2);
++    const auto& l = A(2, 3);
++    const auto& m = A(3, 0);
++    const auto& n = A(3, 1);
++    const auto& o = A(3, 2);
++    const auto& p = A(3, 3);
+ 
+     auto afk = a * f * k;
+     auto afl = a * f * l;
+@@ -232,14 +232,14 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
+     MatrixXv eigen_A{A.rows(), A.cols()};
+     for (int row = 0; row < A.rows(); ++row) {
+       for (int col = 0; col < A.cols(); ++col) {
+-        eigen_A(row, col) = A[row, col];
++        eigen_A(row, col) = A(row, col);
+       }
+     }
+ 
+     MatrixXv eigen_B{B.rows(), B.cols()};
+     for (int row = 0; row < B.rows(); ++row) {
+       for (int col = 0; col < B.cols(); ++col) {
+-        eigen_B(row, col) = B[row, col];
++        eigen_B(row, col) = B(row, col);
+       }
+     }
+ 
+@@ -248,7 +248,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
+     VariableMatrix X{A.cols(), B.cols()};
+     for (int row = 0; row < X.rows(); ++row) {
+       for (int col = 0; col < X.cols(); ++col) {
+-        X[row, col] = eigen_X(row, col);
++        X(row, col) = eigen_X(row, col);
+       }
+     }
+ 

upstream_utils/stb.py

@@ -1,27 +1,27 @@
 #!/usr/bin/env python3
 
-import os
 import shutil
+from pathlib import Path
 
 from upstream_utils import Lib, walk_cwd_and_copy_if
 
 
-def copy_upstream_src(wpilib_root):
-    stb = os.path.join(wpilib_root, "thirdparty", "imgui_suite", "stb")
+def copy_upstream_src(wpilib_root: Path):
+    stb = wpilib_root / "thirdparty/imgui_suite/stb"
 
     # Delete old install
     for d in ["include", "cpp"]:
-        shutil.rmtree(os.path.join(stb, d), ignore_errors=True)
+        shutil.rmtree(stb / d, ignore_errors=True)
 
     # Copy files
-    files = walk_cwd_and_copy_if(
+    walk_cwd_and_copy_if(
         lambda dp, f: f == "stb_image.h",
-        os.path.join(stb, "include"),
+        stb / "include",
     )
 
-    os.makedirs(os.path.join(stb, "cpp"))
+    (stb / "cpp").mkdir(parents=True)
 
-    with open(os.path.join(stb, "cpp", "stb_image.cpp"), "w") as f:
+    with open(stb / "cpp/stb_image.cpp", "w") as f:
         f.write(
             """#define STBI_WINDOWS_UTF8
 #define STB_IMAGE_IMPLEMENTATION

upstream_utils/upstream_utils.py

@@ -5,6 +5,8 @@ import shutil
 import subprocess
 import sys
 import tempfile
+from collections.abc import Callable
+from pathlib import Path
 
 
 def get_repo_root():
@@ -12,15 +14,17 @@ def get_repo_root():
 
     An empty string is returned if no repository root was found.
     """
-    return subprocess.run(
-        ["git", "rev-parse", "--show-toplevel"],
-        stdout=subprocess.PIPE,
-        stderr=subprocess.DEVNULL,
-        encoding="ascii",
-    ).stdout.rstrip()
+    return Path(
+        subprocess.run(
+            ["git", "rev-parse", "--show-toplevel"],
+            stdout=subprocess.PIPE,
+            stderr=subprocess.DEVNULL,
+            encoding="ascii",
+        ).stdout.rstrip()
+    )
 
 
-def walk_if(top, pred):
+def walk_if(top: Path, pred: Callable[[Path, str], bool]):
     """Walks the current directory, then returns a list of files for which the
     given predicate is true.
 
@@ -29,12 +33,10 @@ def walk_if(top, pred):
     pred -- a function that takes a directory path and a filename, then returns
             True if the file should be included in the output list
     """
-    return [
-        os.path.join(dp, f) for dp, dn, fn in os.walk(top) for f in fn if pred(dp, f)
-    ]
+    return [dp / f for dp, dn, fn in top.walk() for f in fn if pred(dp, f)]
 
 
-def copy_to(files, root, rename_c_to_cpp=False):
+def copy_to(files: list[Path], root: Path, rename_c_to_cpp=False):
     """Copies list of files to root by appending the relative paths of the files
     to root.
 
@@ -48,31 +50,33 @@ def copy_to(files, root, rename_c_to_cpp=False):
     Returns:
     The list of files in their destination.
     """
-    if not os.path.exists(root):
-        os.makedirs(root)
+    if not root.exists():
+        root.mkdir(parents=True)
 
-    dest_files = []
+    dest_files: list[Path] = []
     for f in files:
-        dest_file = os.path.join(root, f)
+        dest_file = root / f
 
         # Rename .cc or .cxx file to .cpp
-        if dest_file.endswith(".cc") or dest_file.endswith(".cxx"):
-            dest_file = os.path.splitext(dest_file)[0] + ".cpp"
+        if dest_file.match("*.cc") or dest_file.match("*.cxx"):
+            dest_file = dest_file.with_suffix(".cpp")
 
-        if rename_c_to_cpp and dest_file.endswith(".c"):
-            dest_file = os.path.splitext(dest_file)[0] + ".cpp"
+        if rename_c_to_cpp and dest_file.match("*.c"):
+            dest_file = dest_file.with_suffix(".cpp")
 
         # Make leading directory
-        dest_dir = os.path.dirname(dest_file)
-        if not os.path.exists(dest_dir):
-            os.makedirs(dest_dir)
+        dest_dir = dest_file.parent
+        if not dest_dir.exists():
+            dest_dir.mkdir(parents=True)
 
         shutil.copyfile(f, dest_file)
         dest_files.append(dest_file)
     return dest_files
 
 
-def walk_cwd_and_copy_if(pred, root, rename_c_to_cpp=False):
+def walk_cwd_and_copy_if(
+    pred: Callable[[Path, str], bool], root: Path, rename_c_to_cpp=False
+):
     """Walks the current directory, generates a list of files for which the
     given predicate is true, then copies that list to root by appending the
     relative paths of the files to root.
@@ -88,12 +92,12 @@ def walk_cwd_and_copy_if(pred, root, rename_c_to_cpp=False):
     Returns:
     The list of files in their destination.
     """
-    files = walk_if(".", pred)
+    files = walk_if(Path("."), pred)
     files = copy_to(files, root, rename_c_to_cpp)
     return files
 
 
-def comment_out_invalid_includes(filename, include_roots):
+def comment_out_invalid_includes(filename: Path, include_roots: list[Path]):
     """Comment out #include directives that include a nonexistent file
 
     Keyword arguments:
@@ -114,11 +118,8 @@ def comment_out_invalid_includes(filename, include_roots):
 
         # Comment out #include if the file doesn't exist in current directory or
         # include root
-        if not os.path.exists(
-            os.path.join(os.path.dirname(filename), include)
-        ) and not any(
-            os.path.exists(os.path.join(include_root, include))
-            for include_root in include_roots
+        if not (filename.parent / include).exists() and not any(
+            (include_root / include).exists() for include_root in include_roots
         ):
             new_contents += "// "
 
@@ -135,7 +136,22 @@ def comment_out_invalid_includes(filename, include_roots):
             f.write(new_contents)
 
 
-def git_am(patch, use_threeway=False, ignore_whitespace=False):
+def has_prefix(path: Path, prefix: Path):
+    """Checks if a path has a certain prefix.
+
+    Keyword arguments:
+    path -- path to check if it begins with the prefix
+    prefix -- prefix to use
+
+    Returns:
+    True if the path begins with the prefix, False otherwise.
+    """
+    return len(path.parts) >= len(prefix.parts) and all(
+        p1 == p2 for p1, p2 in zip(path.parts, prefix.parts)
+    )
+
+
+def git_am(patch: Path, use_threeway=False, ignore_whitespace=False):
     """Apply patch to a Git repository in the current directory using "git am".
 
     Keyword arguments:
@@ -152,7 +168,7 @@ def git_am(patch, use_threeway=False, ignore_whitespace=False):
     subprocess.check_output(args + [patch])
 
 
-def has_git_rev(rev):
+def has_git_rev(rev: str):
     """Checks whether the Git repository in the current directory has the given
     revision.
 
@@ -169,10 +185,10 @@ def has_git_rev(rev):
 class Lib:
     def __init__(
         self,
-        name,
-        url,
-        tag,
-        copy_upstream_src,
+        name: str,
+        url: str,
+        tag: str,
+        copy_upstream_src: Callable[[Path], None],
         patch_options={},
         *,
         pre_patch_hook=None,
@@ -207,7 +223,7 @@ class Lib:
         self.pre_patch_commits = pre_patch_commits
         self.wpilib_root = get_repo_root()
 
-    def get_repo_path(self, tempdir=None):
+    def get_repo_path(self, tempdir: str | None = None):
         """Returns the path to the clone of the upstream repository.
 
         Keyword argument:
@@ -221,11 +237,11 @@ class Lib:
         if tempdir is None:
             tempdir = tempfile.gettempdir()
         repo = os.path.basename(self.url)
-        dest = os.path.join(tempdir, repo)
-        dest = dest.removesuffix(".git")
+        dest = Path(tempdir, repo)
+        dest = dest.with_suffix("")
         return dest
 
-    def open_repo(self, *, err_msg_if_absent):
+    def open_repo(self, *, err_msg_if_absent: str | None):
         """Changes the current working directory to the upstream repository. If
         err_msg_if_absent is not None and the upstream repository does not
         exist, the program exits with return code 1.
@@ -241,7 +257,7 @@ class Lib:
 
         print(f"INFO: Opening repository at {dest}")
 
-        if not os.path.exists(dest):
+        if not dest.exists():
             if err_msg_if_absent is None:
                 subprocess.run(["git", "clone", "--filter=tree:0", self.url])
             else:
@@ -285,7 +301,7 @@ class Lib:
             exit(1)
         return root_tags[0]
 
-    def set_root_tag(self, tag):
+    def set_root_tag(self, tag: str):
         """Sets the root tag, deleting any potential candidates first.
 
         Keyword argument:
@@ -307,19 +323,17 @@ class Lib:
         Returns:
         The absolute path to the directory containing the patch files.
         """
-        return os.path.join(self.wpilib_root, f"upstream_utils/{self.name}_patches")
+        return self.wpilib_root / f"upstream_utils/{self.name}_patches"
 
-    def get_patch_list(self):
+    def get_patch_list(self) -> list[Path]:
         """Returns a list of the filenames of the patches to apply.
 
         Returns:
         A list of the filenames of the patches to apply, sorted in lexicographic
         order by the Unicode code points."""
-        if not os.path.exists(self.get_patch_directory()):
+        if not self.get_patch_directory().exists():
             return []
-        return sorted(
-            f for f in os.listdir(self.get_patch_directory()) if f.endswith(".patch")
-        )
+        return sorted(self.get_patch_directory().glob("*.patch"))
 
     def apply_patches(self):
         """Applies the patches listed in the patch list to the current
@@ -330,17 +344,17 @@ class Lib:
 
         for f in self.get_patch_list():
             git_am(
-                os.path.join(self.get_patch_directory(), f),
+                self.get_patch_directory() / f,
                 **self.patch_options,
             )
 
-    def replace_tag(self, tag):
+    def replace_tag(self, tag: str):
         """Replaces the tag in the script.
 
         Keyword argument:
         tag -- The tag to replace the script tag with.
         """
-        path = os.path.join(self.wpilib_root, f"upstream_utils/{self.name}.py")
+        path = self.wpilib_root / f"upstream_utils/{self.name}.py"
         with open(path, "r") as file:
             lines = file.readlines()
 
@@ -391,7 +405,7 @@ class Lib:
 
         self.set_root_tag(self.old_tag)
 
-    def rebase(self, new_tag):
+    def rebase(self, new_tag: str):
         """Rebases the patches.
 
         Keyword argument:
@@ -449,7 +463,7 @@ class Lib:
             ]
         )
 
-        if os.path.exists(self.get_patch_directory()):
+        if self.get_patch_directory().exists():
             shutil.rmtree(self.get_patch_directory())
 
         is_first = True

wpilibNewCommands/src/main/native/include/frc2/command/Command.h

@@ -192,7 +192,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param duration the timeout duration
    * @return the command with the timeout added
    */
-  [[nodiscard]]
   CommandPtr WithTimeout(units::second_t duration) &&;
 
   /**
@@ -203,7 +202,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param condition the interrupt condition
    * @return the command with the interrupt condition added
    */
-  [[nodiscard]]
   CommandPtr Until(std::function<bool()> condition) &&;
 
   /**
@@ -214,7 +212,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param condition the run condition
    * @return the command with the run condition added
    */
-  [[nodiscard]]
   CommandPtr OnlyWhile(std::function<bool()> condition) &&;
 
   /**
@@ -224,7 +221,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr BeforeStarting(std::function<void()> toRun,
                             Requirements requirements = {}) &&;
 
@@ -235,7 +231,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param before the command to run before this one
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr BeforeStarting(CommandPtr&& before) &&;
 
   /**
@@ -245,7 +240,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr AndThen(std::function<void()> toRun,
                      Requirements requirements = {}) &&;
 
@@ -257,7 +251,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param next the commands to run next
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr AndThen(CommandPtr&& next) &&;
 
   /**
@@ -266,7 +259,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    *
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr Repeatedly() &&;
 
   /**
@@ -282,7 +274,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @return the decorated command
    * @see ProxyCommand
    */
-  [[nodiscard]]
   CommandPtr AsProxy() &&;
 
   /**
@@ -294,7 +285,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param condition the condition that will prevent the command from running
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr Unless(std::function<bool()> condition) &&;
 
   /**
@@ -306,7 +296,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param condition the condition that will allow the command to run
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr OnlyIf(std::function<bool()> condition) &&;
 
   /**
@@ -330,7 +319,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @return the decorated command
    * @see WithDeadline
    */
-  [[nodiscard]]
   CommandPtr DeadlineFor(CommandPtr&& parallel) &&;
 
   /**
@@ -341,7 +329,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param parallel the commands to run in parallel
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr AlongWith(CommandPtr&& parallel) &&;
 
   /**
@@ -352,7 +339,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param parallel the commands to run in parallel
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr RaceWith(CommandPtr&& parallel) &&;
 
   /**
@@ -361,7 +347,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param doesRunWhenDisabled true to run when disabled.
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&;
 
   /**
@@ -370,7 +355,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param interruptBehavior the desired interrupt behavior
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr WithInterruptBehavior(
       Command::InterruptionBehavior interruptBehavior) &&;
 
@@ -382,7 +366,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * command was interrupted.
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr FinallyDo(std::function<void(bool)> end) &&;
 
   /**
@@ -394,7 +377,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * interrupted.
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr FinallyDo(std::function<void()> end) &&;
 
   /**
@@ -404,7 +386,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param handler a lambda to run when the command is interrupted
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr HandleInterrupt(std::function<void()> handler) &&;
 
   /**
@@ -413,7 +394,6 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    * @param name name
    * @return the decorated Command
    */
-  [[nodiscard]]
   CommandPtr WithName(std::string_view name) &&;
 
   /**

wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h

@@ -25,7 +25,8 @@ namespace frc2 {
  * std::unique_ptr<Command>, use CommandPtr::Unwrap to convert.
  * CommandPtr::UnwrapVector does the same for vectors.
  */
-class CommandPtr final {
+class [[nodiscard]]
+CommandPtr final {
  public:
   explicit CommandPtr(std::unique_ptr<Command>&& command);
 
@@ -46,7 +47,6 @@ class CommandPtr final {
    *
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr Repeatedly() &&;
 
   /**
@@ -60,7 +60,6 @@ class CommandPtr final {
    * @return the decorated command
    * @see ProxyCommand
    */
-  [[nodiscard]]
   CommandPtr AsProxy() &&;
 
   /**
@@ -69,7 +68,6 @@ class CommandPtr final {
    * @param doesRunWhenDisabled true to run when disabled
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr IgnoringDisable(bool doesRunWhenDisabled) &&;
 
   /**
@@ -78,7 +76,6 @@ class CommandPtr final {
    * @param interruptBehavior the desired interrupt behavior
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr WithInterruptBehavior(
       Command::InterruptionBehavior interruptBehavior) &&;
 
@@ -89,7 +86,6 @@ class CommandPtr final {
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr AndThen(std::function<void()> toRun,
                      Requirements requirements = {}) &&;
 
@@ -101,7 +97,6 @@ class CommandPtr final {
    * @param next the commands to run next
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr AndThen(CommandPtr&& next) &&;
 
   /**
@@ -111,7 +106,6 @@ class CommandPtr final {
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr BeforeStarting(std::function<void()> toRun,
                             Requirements requirements = {}) &&;
 
@@ -122,7 +116,6 @@ class CommandPtr final {
    * @param before the command to run before this one
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr BeforeStarting(CommandPtr&& before) &&;
 
   /**
@@ -133,7 +126,6 @@ class CommandPtr final {
    * @param duration the timeout duration
    * @return the command with the timeout added
    */
-  [[nodiscard]]
   CommandPtr WithTimeout(units::second_t duration) &&;
 
   /**
@@ -144,7 +136,6 @@ class CommandPtr final {
    * @param condition the interrupt condition
    * @return the command with the interrupt condition added
    */
-  [[nodiscard]]
   CommandPtr Until(std::function<bool()> condition) &&;
 
   /**
@@ -155,7 +146,6 @@ class CommandPtr final {
    * @param condition the run condition
    * @return the command with the run condition added
    */
-  [[nodiscard]]
   CommandPtr OnlyWhile(std::function<bool()> condition) &&;
 
   /**
@@ -167,7 +157,6 @@ class CommandPtr final {
    * @param condition the condition that will prevent the command from running
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr Unless(std::function<bool()> condition) &&;
 
   /**
@@ -179,7 +168,6 @@ class CommandPtr final {
    * @param condition the condition that will allow the command to run
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr OnlyIf(std::function<bool()> condition) &&;
 
   /**
@@ -201,7 +189,7 @@ class CommandPtr final {
    * @param parallel the commands to run in parallel
    * @return the decorated command
    */
-  [[nodiscard]] [[deprecated("Replace with DeadlineFor")]]
+  [[deprecated("Replace with DeadlineFor")]]
   CommandPtr DeadlineWith(CommandPtr&& parallel) &&;
 
   /**
@@ -214,7 +202,6 @@ class CommandPtr final {
    * will be interupted when the deadline command ends
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr DeadlineFor(CommandPtr&& parallel) &&;
   /**
    * Decorates this command with a set of commands to run parallel to it, ending
@@ -224,7 +211,6 @@ class CommandPtr final {
    * @param parallel the commands to run in parallel
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr AlongWith(CommandPtr&& parallel) &&;
 
   /**
@@ -235,7 +221,6 @@ class CommandPtr final {
    * @param parallel the commands to run in parallel
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr RaceWith(CommandPtr&& parallel) &&;
 
   /**
@@ -246,7 +231,6 @@ class CommandPtr final {
    * command was interrupted
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr FinallyDo(std::function<void(bool)> end) &&;
 
   /**
@@ -258,7 +242,6 @@ class CommandPtr final {
    * interrupted.
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr FinallyDo(std::function<void()> end) &&;
 
   /**
@@ -268,7 +251,6 @@ class CommandPtr final {
    * @param handler a lambda to run when the command is interrupted
    * @return the decorated command
    */
-  [[nodiscard]]
   CommandPtr HandleInterrupt(std::function<void()> handler) &&;
 
   /**
@@ -278,7 +260,6 @@ class CommandPtr final {
    * @param name name
    * @return the decorated Command
    */
-  [[nodiscard]]
   CommandPtr WithName(std::string_view name) &&;
 
   /**

wpilibNewCommands/src/main/native/include/frc2/command/Commands.h

@@ -29,7 +29,6 @@ namespace cmd {
 /**
  * Constructs a command that does nothing, finishing immediately.
  */
-[[nodiscard]]
 CommandPtr None();
 
 /**
@@ -38,7 +37,6 @@ CommandPtr None();
  * @param requirements Subsystems to require
  * @return the command
  */
-[[nodiscard]]
 CommandPtr Idle(Requirements requirements = {});
 
 // Action Commands
@@ -49,7 +47,6 @@ CommandPtr Idle(Requirements requirements = {});
  * @param action the action to run
  * @param requirements subsystems the action requires
  */
-[[nodiscard]]
 CommandPtr RunOnce(std::function<void()> action,
                    Requirements requirements = {});
 
@@ -59,7 +56,6 @@ CommandPtr RunOnce(std::function<void()> action,
  * @param action the action to run
  * @param requirements subsystems the action requires
  */
-[[nodiscard]]
 CommandPtr Run(std::function<void()> action, Requirements requirements = {});
 
 /**
@@ -70,7 +66,6 @@ CommandPtr Run(std::function<void()> action, Requirements requirements = {});
  * @param end the action to run on interrupt
  * @param requirements subsystems the action requires
  */
-[[nodiscard]]
 CommandPtr StartEnd(std::function<void()> start, std::function<void()> end,
                     Requirements requirements = {});
 
@@ -82,7 +77,6 @@ CommandPtr StartEnd(std::function<void()> start, std::function<void()> end,
  * @param end the action to run on interrupt
  * @param requirements subsystems the action requires
  */
-[[nodiscard]]
 CommandPtr RunEnd(std::function<void()> run, std::function<void()> end,
                   Requirements requirements = {});
 
@@ -94,7 +88,6 @@ CommandPtr RunEnd(std::function<void()> run, std::function<void()> end,
  * @param run the action to run every iteration
  * @param requirements subsystems the action requires
  */
-[[nodiscard]]
 CommandPtr StartRun(std::function<void()> start, std::function<void()> run,
                     Requirements requirements = {});
 
@@ -103,7 +96,6 @@ CommandPtr StartRun(std::function<void()> start, std::function<void()> run,
  *
  * @param msg the message to print
  */
-[[nodiscard]]
 CommandPtr Print(std::string_view msg);
 
 // Idling Commands
@@ -113,7 +105,6 @@ CommandPtr Print(std::string_view msg);
  *
  * @param duration after how long the command finishes
  */
-[[nodiscard]]
 CommandPtr Wait(units::second_t duration);
 
 /**
@@ -122,7 +113,6 @@ CommandPtr Wait(units::second_t duration);
  *
  * @param condition the condition
  */
-[[nodiscard]]
 CommandPtr WaitUntil(std::function<bool()> condition);
 
 // Selector Commands
@@ -134,7 +124,6 @@ CommandPtr WaitUntil(std::function<bool()> condition);
  * @param onFalse the command to run if the selector function returns false
  * @param selector the selector function
  */
-[[nodiscard]]
 CommandPtr Either(CommandPtr&& onTrue, CommandPtr&& onFalse,
                   std::function<bool()> selector);
 
@@ -145,7 +134,6 @@ CommandPtr Either(CommandPtr&& onTrue, CommandPtr&& onFalse,
  * @param commands map of commands to select from
  */
 template <typename Key, std::convertible_to<CommandPtr>... CommandPtrs>
-[[nodiscard]]
 CommandPtr Select(std::function<Key()> selector,
                   std::pair<Key, CommandPtrs>&&... commands) {
   std::vector<std::pair<Key, std::unique_ptr<Command>>> vec;
@@ -162,7 +150,6 @@ CommandPtr Select(std::function<Key()> selector,
  * @param supplier the command supplier
  * @param requirements the set of requirements for this command
  */
-[[nodiscard]]
 CommandPtr Defer(wpi::unique_function<CommandPtr()> supplier,
                  Requirements requirements);
 
@@ -173,7 +160,6 @@ CommandPtr Defer(wpi::unique_function<CommandPtr()> supplier,
  *
  * @param supplier the command supplier
  */
-[[nodiscard]]
 CommandPtr DeferredProxy(wpi::unique_function<Command*()> supplier);
 
 /**
@@ -183,7 +169,6 @@ CommandPtr DeferredProxy(wpi::unique_function<Command*()> supplier);
  *
  * @param supplier the command supplier
  */
-[[nodiscard]]
 CommandPtr DeferredProxy(wpi::unique_function<CommandPtr()> supplier);
 // Command Groups
 
@@ -205,14 +190,12 @@ std::vector<CommandPtr> MakeVector(Args&&... args) {
 /**
  * Runs a group of commands in series, one after the other.
  */
-[[nodiscard]]
 CommandPtr Sequence(std::vector<CommandPtr>&& commands);
 
 /**
  * Runs a group of commands in series, one after the other.
  */
 template <std::convertible_to<CommandPtr>... CommandPtrs>
-[[nodiscard]]
 CommandPtr Sequence(CommandPtrs&&... commands) {
   return Sequence(impl::MakeVector(std::forward<CommandPtrs>(commands)...));
 }
@@ -221,7 +204,6 @@ CommandPtr Sequence(CommandPtrs&&... commands) {
  * Runs a group of commands in series, one after the other. Once the last
  * command ends, the group is restarted.
  */
-[[nodiscard]]
 CommandPtr RepeatingSequence(std::vector<CommandPtr>&& commands);
 
 /**
@@ -229,7 +211,6 @@ CommandPtr RepeatingSequence(std::vector<CommandPtr>&& commands);
  * command ends, the group is restarted.
  */
 template <std::convertible_to<CommandPtr>... CommandPtrs>
-[[nodiscard]]
 CommandPtr RepeatingSequence(CommandPtrs&&... commands) {
   return RepeatingSequence(
       impl::MakeVector(std::forward<CommandPtrs>(commands)...));
@@ -239,7 +220,6 @@ CommandPtr RepeatingSequence(CommandPtrs&&... commands) {
  * Runs a group of commands at the same time. Ends once all commands in the
  * group finish.
  */
-[[nodiscard]]
 CommandPtr Parallel(std::vector<CommandPtr>&& commands);
 
 /**
@@ -247,7 +227,6 @@ CommandPtr Parallel(std::vector<CommandPtr>&& commands);
  * group finish.
  */
 template <std::convertible_to<CommandPtr>... CommandPtrs>
-[[nodiscard]]
 CommandPtr Parallel(CommandPtrs&&... commands) {
   return Parallel(impl::MakeVector(std::forward<CommandPtrs>(commands)...));
 }
@@ -256,7 +235,6 @@ CommandPtr Parallel(CommandPtrs&&... commands) {
  * Runs a group of commands at the same time. Ends once any command in the group
  * finishes, and cancels the others.
  */
-[[nodiscard]]
 CommandPtr Race(std::vector<CommandPtr>&& commands);
 
 /**
@@ -264,7 +242,6 @@ CommandPtr Race(std::vector<CommandPtr>&& commands);
  * finishes, and cancels the others.
  */
 template <std::convertible_to<CommandPtr>... CommandPtrs>
-[[nodiscard]]
 CommandPtr Race(CommandPtrs&&... commands) {
   return Race(impl::MakeVector(std::forward<CommandPtrs>(commands)...));
 }
@@ -273,7 +250,6 @@ CommandPtr Race(CommandPtrs&&... commands) {
  * Runs a group of commands at the same time. Ends once a specific command
  * finishes, and cancels the others.
  */
-[[nodiscard]]
 CommandPtr Deadline(CommandPtr&& deadline, std::vector<CommandPtr>&& others);
 
 /**
@@ -281,7 +257,6 @@ CommandPtr Deadline(CommandPtr&& deadline, std::vector<CommandPtr>&& others);
  * finishes, and cancels the others.
  */
 template <std::convertible_to<CommandPtr>... CommandPtrs>
-[[nodiscard]]
 CommandPtr Deadline(CommandPtr&& deadline, CommandPtrs&&... commands) {
   return Deadline(std::move(deadline),
                   impl::MakeVector(std::forward<CommandPtrs>(commands)...));

wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h

@@ -127,7 +127,6 @@ class Subsystem {
    *
    * @param action the action to run
    */
-  [[nodiscard]]
   CommandPtr RunOnce(std::function<void()> action);
 
   /**
@@ -136,7 +135,6 @@ class Subsystem {
    *
    * @param action the action to run
    */
-  [[nodiscard]]
   CommandPtr Run(std::function<void()> action);
 
   /**
@@ -146,7 +144,6 @@ class Subsystem {
    * @param start the action to run on start
    * @param end the action to run on interrupt
    */
-  [[nodiscard]]
   CommandPtr StartEnd(std::function<void()> start, std::function<void()> end);
 
   /**
@@ -156,7 +153,6 @@ class Subsystem {
    * @param run the action to run every iteration
    * @param end the action to run on interrupt
    */
-  [[nodiscard]]
   CommandPtr RunEnd(std::function<void()> run, std::function<void()> end);
 
   /**
@@ -166,7 +162,6 @@ class Subsystem {
    * @param start the action to run on start
    * @param run the action to run every iteration
    */
-  [[nodiscard]]
   CommandPtr StartRun(std::function<void()> start, std::function<void()> run);
 
   /**
@@ -176,7 +171,6 @@ class Subsystem {
    * @param supplier the command supplier.
    * @return the command.
    */
-  [[nodiscard]]
   CommandPtr Defer(wpi::unique_function<CommandPtr()> supplier);
 };
 }  // namespace frc2

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp

@@ -38,5 +38,6 @@ TEST_F(CommandPtrTest, MovedFrom) {
 }
 
 TEST_F(CommandPtrTest, NullInitialization) {
-  EXPECT_THROW(CommandPtr{std::unique_ptr<Command>{}}, frc::RuntimeError);
+  EXPECT_THROW(auto cmd = CommandPtr{std::unique_ptr<Command>{}},
+               frc::RuntimeError);
 }

wpilibc/src/main/native/include/frc/AnalogAccelerometer.h

@@ -75,8 +75,7 @@ class AnalogAccelerometer : public wpi::Sendable,
    * Set the accelerometer sensitivity.
    *
    * This sets the sensitivity of the accelerometer used for calculating the
-   * acceleration. The sensitivity varies by accelerometer model. There are
-   * constants defined for various models.
+   * acceleration. The sensitivity varies by accelerometer model.
    *
    * @param sensitivity The sensitivity of accelerometer in Volts per G.
    */
@@ -85,8 +84,7 @@ class AnalogAccelerometer : public wpi::Sendable,
   /**
    * Set the voltage that corresponds to 0 G.
    *
-   * The zero G voltage varies by accelerometer model. There are constants
-   * defined for various models.
+   * The zero G voltage varies by accelerometer model.
    *
    * @param zero The zero G voltage.
    */

wpilibc/src/main/native/include/frc/event/BooleanEvent.h

@@ -129,8 +129,12 @@ class BooleanEvent {
                             frc::Debouncer::DebounceType::kRising);
 
  private:
+  /// Poller loop.
   EventLoop* m_loop;
+
   std::function<bool()> m_signal;
-  std::shared_ptr<bool> m_state;  // A programmer's worst nightmare.
+
+  /// The state of the condition in the current loop poll.
+  std::shared_ptr<bool> m_state;
 };
 }  // namespace frc

wpilibcExamples/CMakeLists.txt

@@ -9,6 +9,7 @@ subdir_list(SNIPPETS ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/snippets)
 
 add_custom_target(wpilibcExamples)
 add_custom_target(wpilibcExamples_test)
+add_custom_target(wpilibcExamples_snippets)
 
 foreach(example ${EXAMPLES})
     file(
@@ -72,8 +73,8 @@ foreach(snippet ${SNIPPETS})
         src/main/cpp/snippets/${snippet}/c/*.c
     )
     add_executable(snippet${snippet} ${sources})
-    wpilib_target_warnings(${snippet})
+    wpilib_target_warnings(snippet${snippet})
     target_include_directories(snippet${snippet} PUBLIC src/main/cpp/snippets/${snippet}/include)
     target_link_libraries(snippet${snippet} wpilibc wpilibNewCommands romiVendordep xrpVendordep)
-    add_dependencies(wpilibcExamples snippet${snippet})
+    add_dependencies(wpilibcExamples_snippets snippet${snippet})
 endforeach()

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h

@@ -82,7 +82,6 @@ class DriveSubsystem : public frc2::SubsystemBase {
    * @param distance The distance to drive forward.
    * @return A command.
    */
-  [[nodiscard]]
   frc2::CommandPtr ProfiledDriveDistance(units::meter_t distance);
 
   /**
@@ -92,7 +91,6 @@ class DriveSubsystem : public frc2::SubsystemBase {
    * @param distance The distance to drive forward.
    * @return A command.
    */
-  [[nodiscard]]
   frc2::CommandPtr DynamicProfiledDriveDistance(units::meter_t distance);
 
  private:

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h

@@ -28,7 +28,6 @@ class Drive : public frc2::SubsystemBase {
    * @param fwd the commanded forward movement
    * @param rot the commanded rotation
    */
-  [[nodiscard]]
   frc2::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
                                       std::function<double()> rot);
 
@@ -39,7 +38,6 @@ class Drive : public frc2::SubsystemBase {
    * @param distance The distance to drive forward in meters
    * @param speed The fraction of max speed at which to drive
    */
-  [[nodiscard]]
   frc2::CommandPtr DriveDistanceCommand(units::meter_t distance, double speed);
 
   /**
@@ -48,7 +46,6 @@ class Drive : public frc2::SubsystemBase {
    *
    * @param angle The angle to turn to
    */
-  [[nodiscard]]
   frc2::CommandPtr TurnToAngleCommand(units::degree_t angle);
 
  private:

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h

@@ -19,11 +19,9 @@ class Intake : public frc2::SubsystemBase {
 
   /** Returns a command that deploys the intake, and then runs the intake motor
    * indefinitely. */
-  [[nodiscard]]
   frc2::CommandPtr IntakeCommand();
 
   /** Returns a command that turns off and retracts the intake. */
-  [[nodiscard]]
   frc2::CommandPtr RetractCommand();
 
  private:

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Pneumatics.h

@@ -17,7 +17,6 @@ class Pneumatics : frc2::SubsystemBase {
  public:
   Pneumatics();
   /** Returns a command that disables the compressor indefinitely. */
-  [[nodiscard]]
   frc2::CommandPtr DisableCompressorCommand();
 
   /**

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h

@@ -28,7 +28,6 @@ class Shooter : public frc2::SubsystemBase {
    *
    * @param setpointRotationsPerSecond The desired shooter velocity
    */
-  [[nodiscard]]
   frc2::CommandPtr ShootCommand(units::turns_per_second_t setpoint);
 
  private:

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.h

@@ -16,7 +16,6 @@ class Storage : frc2::SubsystemBase {
  public:
   Storage();
   /** Returns a command that runs the storage motor indefinitely. */
-  [[nodiscard]]
   frc2::CommandPtr RunCommand();
 
   /** Whether the ball storage is full. */

wpilibcExamples/src/main/cpp/snippets/AnalogAccelerometer/cpp/Robot.cpp

@@ -0,0 +1,36 @@
+// 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 <frc/AnalogAccelerometer.h>
+#include <frc/AnalogInput.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * AnalogAccelerometer snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/accelerometers-software.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  Robot() {
+    // Sets the sensitivity of the accelerometer to 1 volt per G
+    m_accelerometer.SetSensitivity(1);
+    // Sets the zero voltage of the accelerometer to 3 volts
+    m_accelerometer.SetZero(3);
+  }
+
+  void TeleopPeriodic() override {
+    // Gets the current acceleration
+    m_accelerometer.GetAcceleration();
+  }
+
+ private:
+  // Creates an analog accelerometer on analog input 0
+  frc::AnalogAccelerometer m_accelerometer{0};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/AnalogEncoder/cpp/Robot.cpp

@@ -0,0 +1,35 @@
+// 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 <frc/AnalogEncoder.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * AnalogEncoder snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  Robot() {}
+
+  void TeleopPeriodic() override {
+    // Gets the rotation
+    m_encoder.Get();
+  }
+
+ private:
+  // Initializes an analog encoder on Analog Input pin 0
+  frc::AnalogEncoder m_encoder{0};
+
+  // Initializes an analog encoder on DIO pins 0 to return a value of 4 for
+  // a full rotation, with the encoder reporting 0 half way through rotation (2
+  // out of 4)
+  frc::AnalogEncoder m_encoderFR{0, 4.0, 2.0};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/AnalogInput/cpp/Robot.cpp

@@ -0,0 +1,59 @@
+// 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 <frc/AnalogInput.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * AnalogInput snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/analog-input-software.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  Robot() {
+    // Sets the AnalogInput to 4-bit oversampling.  16 samples will be added
+    // together.
+    // Thus, the reported values will increase by about a factor of 16, and the
+    // update rate will decrease by a similar amount.
+    m_analog.SetOversampleBits(4);
+
+    // Sets the AnalogInput to 4-bit averaging.  16 samples will be averaged
+    // together. The update rate will decrease by a factor of 16.
+    m_analog.SetAverageBits(4);
+
+    // Gets the raw instantaneous measured value from the analog input, without
+    // applying any calibration and ignoring oversampling and averaging
+    // settings.
+    m_analog.GetValue();
+
+    // Gets the instantaneous measured voltage from the analog input.
+    // Oversampling and averaging settings are ignored
+    m_analog.GetVoltage();
+
+    // Gets the averaged value from the analog input.  The value is not
+    // rescaled, but oversampling and averaging are both applied.
+    m_analog.GetAverageValue();
+
+    // Gets the averaged voltage from the analog input.  Rescaling,
+    // oversampling, and averaging are all applied.
+    m_analog.GetAverageVoltage();
+  }
+
+  void TeleopPeriodic() override {
+  }
+
+ private:
+  // Initializes an AnalogInput on port 0
+  frc::AnalogInput m_analog{0};
+
+  // The count and value variables to fill
+  int64_t count;
+  int64_t value;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/AnalogPotentiometer/cpp/Robot.cpp

@@ -0,0 +1,45 @@
+// 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 <frc/AnalogInput.h>
+#include <frc/AnalogPotentiometer.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * AnalogPotentiometer snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/analog-potentiometers-software.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  Robot() {
+    // Set averaging bits to 2
+    m_input.SetAverageBits(2);
+  }
+
+  void TeleopPeriodic() override {
+    // Get the value of the potentiometer
+    m_pot.Get();
+  }
+
+ private:
+  // Initializes an AnalogPotentiometer on analog port 0
+  // The full range of motion (in meaningful external units) is 0-180 (this
+  // could be degrees, for instance) The "starting point" of the motion, i.e.
+  // where the mechanism is located when the potentiometer reads 0v, is 30.
+  frc::AnalogPotentiometer m_pot{0, 180, 30};
+
+  // Initializes an AnalogInput on port 1
+  frc::AnalogInput m_input{1};
+  // Initializes an AnalogPotentiometer with the given AnalogInput
+  // The full range of motion (in meaningful external units) is 0-180 (this
+  // could be degrees, for instance) The "starting point" of the motion, i.e.
+  // where the mechanism is located when the potentiometer reads 0v, is 30.
+  frc::AnalogPotentiometer m_pot1{&m_input, 180, 30};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/DigitalInput/cpp/Robot.cpp

@@ -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 <frc/DigitalInput.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * Digital Input snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/digital-input-software.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void TeleopPeriodic() override {
+    // Gets the value of the digital input.  Returns true if the circuit is
+    // open.
+    m_input.Get();
+  }
+
+ private:
+  // Initializes a DigitalInput on DIO 0
+  frc::DigitalInput m_input{0};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/DutyCycleEncoder/cpp/Robot.cpp

@@ -2,8 +2,6 @@
 // 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 <numbers>
-
 #include <frc/DutyCycleEncoder.h>
 #include <frc/TimedRobot.h>
 

wpilibcExamples/src/main/cpp/snippets/Encoder/cpp/Robot.cpp

@@ -2,11 +2,8 @@
 // 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 <numbers>
-
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
-#include <frc/smartdashboard/SmartDashboard.h>
 #include <wpi/deprecated.h>
 
 /**

wpilibcExamples/src/main/cpp/snippets/EncoderDrive/cpp/Robot.cpp

@@ -0,0 +1,54 @@
+// 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 <frc/Encoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/Spark.h>
+
+/**
+ * Encoder drive to distance snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  Robot() {
+    // Configures the encoder's distance-per-pulse
+    // The robot moves forward 1 foot per encoder rotation
+    // There are 256 pulses per encoder rotation
+    m_encoder.SetDistancePerPulse(1.0 / 256.0);
+    // Invert the right side of the drivetrain. You might have to invert the
+    // other side
+    rightLeader.SetInverted(true);
+    // Configure the followers to follow the leaders
+    leftLeader.AddFollower(leftFollower);
+    rightLeader.AddFollower(rightFollower);
+  }
+  void AutonomousPeriodic() override {
+    // Drives forward at half speed until the robot has moved 5 feet, then
+    // stops:
+    if (m_encoder.GetDistance() < 5) {
+      drive.TankDrive(0.5, 0.5);
+    } else {
+      drive.TankDrive(0, 0);
+    }
+  }
+
+ private:
+  // Creates an encoder on DIO ports 0 and 1.
+  frc::Encoder m_encoder{0, 1};
+  // Initialize motor controllers and drive
+  frc::Spark leftLeader{0};
+  frc::Spark leftFollower{1};
+  frc::Spark rightLeader{2};
+  frc::Spark rightFollower{3};
+  frc::DifferentialDrive drive{[&](double output) { leftLeader.Set(output); },
+                               [&](double output) { rightLeader.Set(output); }};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/EncoderHoming/cpp/Robot.cpp

@@ -0,0 +1,38 @@
+// 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 <frc/DigitalInput.h>
+#include <frc/Encoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/motorcontrol/Spark.h>
+
+/**
+ * Encoder mechanism homing snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/encoders-software.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void AutonomousPeriodic() override {
+    // Runs the motor backwards at half speed until the limit switch is pressed
+    // then turn off the motor and reset the encoder
+    if (!m_limit.Get()) {
+      m_spark.Set(-0.5);
+    } else {
+      m_spark.Set(0);
+      m_encoder.Reset();
+    }
+  }
+
+ private:
+  frc::Encoder m_encoder{0, 1};
+  frc::Spark m_spark{0};
+  // Limit switch on DIO 2
+  frc::DigitalInput m_limit{2};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/LimitSwitch/cpp/Robot.cpp

@@ -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 <frc/DigitalInput.h>
+#include <frc/Encoder.h>
+#include <frc/Joystick.h>
+#include <frc/TimedRobot.h>
+#include <frc/motorcontrol/PWMVictorSPX.h>
+
+/**
+ * Limit Switch snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/limit-switch.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void TeleopPeriodic() override { SetMotorSpeed(m_joystick.GetRawAxis(2)); }
+  void SetMotorSpeed(double speed) {
+    if (speed > 0) {
+      if (m_toplimitSwitch.Get()) {
+        // We are going up and top limit is tripped so stop
+        m_motor.Set(0);
+      } else {
+        // We are going up but top limit is not tripped so go at commanded speed
+        m_motor.Set(speed);
+      }
+    } else {
+      if (m_bottomlimitSwitch.Get()) {
+        // We are going down and bottom limit is tripped so stop
+        m_motor.Set(0);
+      } else {
+        // We are going down but bottom limit is not tripped so go at commanded
+        // speed
+        m_motor.Set(speed);
+      }
+    }
+  }
+
+ private:
+  frc::DigitalInput m_toplimitSwitch{0};
+  frc::DigitalInput m_bottomlimitSwitch{1};
+  frc::PWMVictorSPX m_motor{0};
+  frc::Joystick m_joystick{0};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/snippets.json

@@ -19,5 +19,89 @@
     ],
     "foldername": "DutyCycleEncoder",
     "gradlebase": "cpp"
+  },
+  {
+    "name": "AnalogEncoder",
+    "description": "Snippets of AnalogEncoder class usage for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Encoder",
+      "Analog"
+    ],
+    "foldername": "AnalogEncoder",
+    "gradlebase": "cpp"
+  },
+  {
+    "name": "EncoderDrive",
+    "description": "Snippets of driving to a distance for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Encoder",
+      "Differential Drive"
+    ],
+    "foldername": "EncoderDrive",
+    "gradlebase": "cpp"
+  },
+  {
+    "name": "EncoderHoming",
+    "description": "Snippets of homing a mechanism for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Encoder",
+      "Digital Input"
+    ],
+    "foldername": "EncoderHoming",
+    "gradlebase": "cpp"
+  },
+  {
+    "name": "LimitSwitch",
+    "description": "Snippets of Limit Switch for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Digital Input"
+    ],
+    "foldername": "LimitSwitch",
+    "gradlebase": "cpp"
+  },
+  {
+    "name": "DigitalInput",
+    "description": "Snippets of Digital Input for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Digital Input"
+    ],
+    "foldername": "DigitalInput",
+    "gradlebase": "cpp"
+  },
+  {
+    "name": "AnalogInput",
+    "description": "Snippets of Analog Input for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Analog"
+    ],
+    "foldername": "AnalogInput",
+    "gradlebase": "cpp"
+  },
+  {
+    "name": "AnalogPotentiometer",
+    "description": "Snippets of Analog Potentiometer for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Analog"
+    ],
+    "foldername": "AnalogPotentiometer",
+    "gradlebase": "cpp"
+  },
+  {
+    "name": "AnalogAccelerometer",
+    "description": "Snippets of Analog Accelerometer for frc-docs.",
+    "tags": [
+      "Hardware",
+      "Analog",
+      "Accelerometer"
+    ],
+    "foldername": "AnalogAccelerometer",
+    "gradlebase": "cpp"
   }
 ]

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java

@@ -90,7 +90,7 @@ public class AnalogAccelerometer implements Sendable, AutoCloseable {
    * Set the accelerometer sensitivity.
    *
    * <p>This sets the sensitivity of the accelerometer used for calculating the acceleration. The
-   * sensitivity varies by accelerometer model. There are constants defined for various models.
+   * sensitivity varies by accelerometer model.
    *
    * @param sensitivity The sensitivity of accelerometer in Volts per G.
    */
@@ -101,8 +101,7 @@ public class AnalogAccelerometer implements Sendable, AutoCloseable {
   /**
    * Set the voltage that corresponds to 0 G.
    *
-   * <p>The zero G voltage varies by accelerometer model. There are constants defined for various
-   * models.
+   * <p>The zero G voltage varies by accelerometer model.
    *
    * @param zero The zero G voltage.
    */

wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java

@@ -28,7 +28,7 @@ public class BooleanEvent implements BooleanSupplier {
 
   private final BooleanSupplier m_signal;
 
-  /** The state of the condition in the current loop poll. Nightmare to manage. */
+  /** The state of the condition in the current loop poll. */
   private final AtomicBoolean m_state = new AtomicBoolean(false);
 
   /**

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogaccelerometer/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.snippets.analogaccelerometer;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/analogaccelerometer/Robot.java

@@ -0,0 +1,31 @@
+// 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.
+
+package edu.wpi.first.wpilibj.snippets.analogaccelerometer;
+
+import edu.wpi.first.wpilibj.AnalogAccelerometer;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+/**
+ * AnalogAccelerometer snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/hardware-apis/sensors/accelerometers-software.html
+ */
+public class Robot extends TimedRobot {
+  // Creates an analog accelerometer on analog input 0
+  AnalogAccelerometer m_accelerometer = new AnalogAccelerometer(0);
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    // Sets the sensitivity of the accelerometer to 1 volt per G
+    m_accelerometer.setSensitivity(1);
+    // Sets the zero voltage of the accelerometer to 3 volts
+    m_accelerometer.setZero(3);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Gets the current acceleration
+    m_accelerometer.getAcceleration();
+  }
+}