Merge pull request #530 from truher/main

mostly GTSAM pose estimator
Team100 · Oct 30, 2024 · 9bd9400 · 9bd9400
2 parents debb53d + 64249da
commit 9bd9400
Show file tree

Hide file tree

Showing 49 changed files with 3,762 additions and 345 deletions.
diff --git a/comp/swerve100/swerve100.code-workspace b/comp/swerve100/swerve100.code-workspace
@@ -13,7 +13,7 @@
         "terminal.integrated.scrollback": 100000,
         "workbench.colorTheme": "Default Light+",
         "files.autoSave": "afterDelay",
-        "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx16G -Xms100m -Xlog:disable",
+        "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx32G -Xms100m -Xlog:disable",
         "workbench.editor.revealIfOpen": true,
         "editor.minimap.enabled": false,
         "editor.inlayHints.enabled": "off",

diff --git a/comp/swerve100/vendordeps/ReduxLib_2024.json b/comp/swerve100/vendordeps/ReduxLib_2024.json
@@ -1,55 +1,55 @@
 {
-  "fileName": "ReduxLib_2024.json",
-  "name": "ReduxLib",
-  "version": "2024.3.0",
-  "frcYear": 2024,
-  "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
-  "mavenUrls": [
-    "https://maven.reduxrobotics.com/"
-  ],
-  "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json",
-  "javaDependencies": [
-    {
-      "groupId": "com.reduxrobotics.frc",
-      "artifactId": "ReduxLib-java",
-      "version": "2024.3.0"
-    }
-  ],
-  "jniDependencies": [
-    {
-      "groupId": "com.reduxrobotics.frc",
-      "artifactId": "ReduxLib-driver",
-      "version": "2024.3.0",
-      "isJar": false,
-      "skipInvalidPlatforms": true,
-      "validPlatforms": [
-        "linuxathena",
-        "linuxx86-64",
-        "linuxarm32",
-        "linuxarm64",
-        "osxuniversal",
-        "windowsx86-64"
-      ]
-    }
-  ],
-  "cppDependencies": [
-    {
-      "groupId": "com.reduxrobotics.frc",
-      "artifactId": "ReduxLib-cpp",
-      "version": "2024.3.0",
-      "libName": "ReduxLib-cpp",
-      "headerClassifier": "headers",
-      "sourcesClassifier": "sources",
-      "sharedLibrary": true,
-      "skipInvalidPlatforms": true,
-      "binaryPlatforms": [
-        "linuxathena",
-        "linuxx86-64",
-        "linuxarm32",
-        "linuxarm64",
-        "osxuniversal",
-        "windowsx86-64"
-      ]
-    }
-  ]
+    "fileName": "ReduxLib_2024.json",
+    "name": "ReduxLib",
+    "version": "2024.3.1",
+    "frcYear": 2024,
+    "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
+    "mavenUrls": [
+        "https://maven.reduxrobotics.com/"
+    ],
+    "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.reduxrobotics.frc",
+            "artifactId": "ReduxLib-java",
+            "version": "2024.3.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.reduxrobotics.frc",
+            "artifactId": "ReduxLib-driver",
+            "version": "2024.3.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxx86-64",
+                "linuxarm32",
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.reduxrobotics.frc",
+            "artifactId": "ReduxLib-cpp",
+            "version": "2024.3.1",
+            "libName": "ReduxLib-cpp",
+            "headerClassifier": "headers",
+            "sourcesClassifier": "sources",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxx86-64",
+                "linuxarm32",
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ]
 }
diff --git a/lib/src/main/java/org/team100/lib/localization/SwerveDrivePoseEstimator100.java b/lib/src/main/java/org/team100/lib/localization/SwerveDrivePoseEstimator100.java
@@ -14,6 +14,7 @@
 import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeDelta;
 import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleDelta;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePosition100;
 import org.team100.lib.util.DriveUtil;
 import org.team100.lib.util.Util;
@@ -202,13 +203,14 @@ public void put(
 
         // the entry right before this one, the basis for integration.
         Entry<Double, InterpolationRecord> lowerEntry = consistentPair.get(0);
-        // System.out.println("SwerveDrivePoseEstiamtor.put() lowerEntry " + lowerEntry);
+        // System.out.println("SwerveDrivePoseEstiamtor.put() lowerEntry " +
+        // lowerEntry);
 
         double t1 = currentTimeS - lowerEntry.getKey();
         InterpolationRecord value = lowerEntry.getValue();
         SwerveState previousState = value.m_state;
 
-        SwerveModulePosition100[] modulePositionDelta = DriveUtil.modulePositionDelta(
+        SwerveModuleDelta[] modulePositionDelta = DriveUtil.modulePositionDelta(
                 value.m_wheelPositions,
                 wheelPositions);
 
@@ -231,7 +233,8 @@ public void put(
                 deltaTransform.getX(),
                 deltaTransform.getY(),
                 deltaTransform.getRotation().getRadians());
-        // System.out.println("SwerveDrivePoseEstimator.put() current velocity " + velocity);
+        // System.out.println("SwerveDrivePoseEstimator.put() current velocity " +
+        // velocity);
 
         // calculate acceleration if possible
         FieldRelativeAcceleration accel = new FieldRelativeAcceleration(0, 0, 0);
@@ -246,7 +249,8 @@ public void put(
             // for acceleration we recalculate from position, since position might have been
             // updated by the cameras.
             Map.Entry<Double, InterpolationRecord> earlierEntry = consistentPair.get(1);
-            // System.out.println("SwerveDrivePoseEstiamtor.put() earlierEntry " + earlierEntry);
+            // System.out.println("SwerveDrivePoseEstiamtor.put() earlierEntry " +
+            // earlierEntry);
 
             double t0 = lowerEntry.getKey() - earlierEntry.getKey();
             // System.out.println("SwerveDrivePoseEstimator.put() accel " + accel);