fix comp bot, also factor tests

comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpFeeder.java

@@ -21,7 +21,7 @@ public class AmpFeeder extends SubsystemBase implements Glassy {
     public AmpFeeder(LoggerFactory parent) {
         LoggerFactory child = parent.child(this);
         switch (Identity.instance) {
-            case COMP_BOT:
+            case DISABLED:
                 Feedforward100 ff = Feedforward100.makeNeo();
                 PIDConstants pid = new PIDConstants(0, 0, 0);
                 ampDrive = new NeoCANSparkMotor(child, 33, MotorPhase.FORWARD, 40, ff, pid);

comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/AmpPivot.java

@@ -67,7 +67,7 @@ public AmpPivot(LoggerFactory parent) {
         controller.setIntegratorRange(0, 0.1);
 
         switch (Identity.instance) {
-            case COMP_BOT:
+            case DISABLED:
                 Feedforward100 ff = Feedforward100.makeArmPivot();
                 PIDConstants pid = new PIDConstants(kOutboardP, kOutboardI, kOutboardD);
                 CANSparkMotor motor = new NeoCANSparkMotor(

comp/swerve100/src/main/java/org/team100/frc2024/motion/intake/Intake.java

@@ -48,7 +48,7 @@ public Intake(LoggerFactory parent, SensorInterface sensors) {
         m_sensors = sensors;
 
         switch (Identity.instance) {
-            case COMP_BOT:
+            case DISABLED:
                 m_intake = new PWMSparkMax(1);
                 m_centering = new PWMSparkMax(2);
                 superRollers = ServoFactory.limitedNeoVelocityServo(

comp/swerve100/src/main/java/org/team100/frc2024/motion/shooter/DrumShooter.java

@@ -120,7 +120,7 @@ public DrumShooter(
         Feedforward100 rollerFF = Feedforward100.makeShooterFalcon6();
 
         switch (Identity.instance) {
-            case COMP_BOT:
+            case DISABLED:
                 Falcon6Motor leftMotor = new Falcon6Motor(
                         leftLogger,
                         leftID,

lib/src/main/java/org/team100/lib/config/Identity.java

@@ -28,6 +28,7 @@ public enum Identity {
     SWERVE_TWO("0317f285"),
     COMP_BOT("03238232"), 
     SWERVE_ONE("032363AC"), 
+    DISABLED("disabled"), // for mechanisms which don't exist
     BLANK(""), // e.g. test default or simulation
     UNKNOWN(null);
 

lib/src/main/java/org/team100/lib/encoder/AnalogTurningEncoder.java

@@ -33,9 +33,9 @@ public AnalogTurningEncoder(
         m_input = new AnalogInput(channel);
         m_voltage = Memo.ofDouble(m_input::getVoltage);
         m_rail = Memo.ofDouble(RobotController::getVoltage5V);
-        child.intLogger(Level.TRACE, "channel").log(m_input::getChannel);
         m_log_voltage = child.doubleLogger(Level.TRACE, "voltage");
         m_log_ratio = child.doubleLogger(Level.TRACE, "ratio");
+        child.intLogger(Level.COMP, "channel").log(m_input::getChannel);
     }
 
     @Override

lib/src/main/java/org/team100/lib/encoder/DutyCycleRotaryPositionSensor.java

@@ -42,7 +42,7 @@ protected DutyCycleRotaryPositionSensor(
         m_dutyCycle = new DutyCycle(m_digitalInput);
         m_duty = Memo.ofDouble(m_dutyCycle::getOutput);
         m_log_duty = child.doubleLogger(Level.TRACE, "duty cycle");
-        child.intLogger(Level.TRACE, "channel").log(() -> channel);
+        child.intLogger(Level.COMP, "channel").log(() -> channel);
     }
 
     @Override

lib/src/test/java/org/team100/lib/swerve/SwerveUtilTest.java

@@ -207,4 +207,18 @@ void testMinAccel() {
         double accelLimit = SwerveUtil.minAccel(limits, 0.92);
         assertEquals(0.8, accelLimit, kDelta);
     }
+
+    @Test
+    void simMinAccel() {
+        // simulate full-throttle to see the exponential curve. 
+        // https://docs.google.com/spreadsheets/d/1k-g8_blQP3X1RNtjFQgk1CJXyzzvNLNuUbWhaNLduvw
+        SwerveKinodynamics limits = SwerveKinodynamicsFactory.forRealisticTest();
+        double v = 0;
+        final double dt = 0.02;
+        for (double t = 0; t < 3; t += dt) {
+            System.out.printf("%5.3f %5.3f\n", t, v);
+            double a = SwerveUtil.minAccel(limits, v);
+            v += dt * a;
+        }
+    }
 }

raspberry_pi/app/pose_estimator/calibrate.py

@@ -114,6 +114,12 @@
 # TODO: if you want to model *blur* then you need more noise here, and maybe more in x than y.
 PX_NOISE = noiseModel.Diagonal.Sigmas(np.array([1, 1]))
 
+# the "batch" mode uses a python CustomFactor that takes multiple landmarks
+# and corresponding pixels; the python stuff is very slow.
+# the "not batch" mode uses a C++ factor, PlanarProjectionFactor,
+# which takes a single landmark at a time (because there's no particular
+# performance advantage to batching in C++)
+USE_BATCH = False
 
 class Calibrate:
     def __init__(self, lag_s: float) -> None:
@@ -262,12 +268,27 @@ def apriltag_for_calibration(
     def apriltag_for_calibration_batch(
         self, landmarks: list[np.ndarray], measured: np.ndarray, t0_us: int
     ) -> None:
-        noise = noiseModel.Diagonal.Sigmas(np.concatenate([[1, 1] for _ in landmarks]))
-        self._new_factors.push_back(
-            apriltag_calibrate_batch.factor(
-                landmarks, measured, noise, X(t0_us), C(0), K(0)
+        """With constant offset and calibration, either uses a python CustomFactor
+        to solve a batch at once, or, uses multiple C++ factors.
+        landmarks: list of 3d points
+        measured: concatenated px measurements
+        TODO: flatten landmarks"""
+        if USE_BATCH:
+            noise = noiseModel.Diagonal.Sigmas(np.concatenate([[1, 1] for _ in landmarks]))
+            self._new_factors.push_back(
+                apriltag_calibrate_batch.factor(
+                    landmarks, measured, noise, X(t0_us), C(0), K(0)
+                )
             )
-        )
+        else:
+            for i, landmark in enumerate(landmarks):
+                px = measured[i * 2 : (i + 1) * 2]
+                noise = noiseModel.Diagonal.Sigmas(np.array([1, 1]))
+                self._new_factors.push_back(
+                    apriltag_calibrate.factor(
+                        landmark, px, noise, X(t0_us), C(0), K(0)
+                    )
+                )   
 
     def keep_calib_hot(self, t0_us: int) -> None:
         """Even if we don't see any targets, remember the

raspberry_pi/app/pose_estimator/estimate.py

@@ -14,6 +14,7 @@
 from wpimath.geometry import Pose2d, Rotation2d, Translation2d, Twist2d
 
 import app.pose_estimator.factors.apriltag_smooth_batch as apriltag_smooth_batch
+import app.pose_estimator.factors.apriltag_smooth as apriltag_smooth
 import app.pose_estimator.factors.gyro as gyro
 import app.pose_estimator.factors.odometry as odometry
 from app.util.drive_util import DriveUtil
@@ -41,13 +42,20 @@
 # TODO: if you want to model *blur* then you need more noise here, and maybe more in x than y.
 PX_NOISE = noiseModel.Diagonal.Sigmas(np.array([1, 1]))
 
+# the "batch" mode uses a python CustomFactor that takes multiple landmarks
+# and corresponding pixels; the python stuff is very slow.
+# the "not batch" mode uses a C++ factor, PlanarProjectionFactor,
+# which takes a single landmark at a time (because there's no particular
+# performance advantage to batching in C++)
+USE_BATCH = False
+
 
 class Estimate:
-    def __init__(self) -> None:
+    def __init__(self, lag: float) -> None:
         """Initialize the model
         initial module positions are at their origins.
         TODO: some other initial positions?"""
-        self._isam: gtsam.BatchFixedLagSmoother = make_smoother(0.1)
+        self._isam: gtsam.BatchFixedLagSmoother = make_smoother(lag)
         self._result: gtsam.Values = gtsam.Values()
         # between updates we accumulate inputs here
         self._new_factors = gtsam.NonlinearFactorGraph()
@@ -186,16 +194,29 @@ def apriltag_for_smoothing_batch(
         camera_offset: gtsam.Pose3,
         calib: gtsam.Cal3DS2,
     ) -> None:
-        """With constant offset and calibration, solves a batch of landmarks at once.
+        """With constant offset and calibration, either uses a python CustomFactor
+         to solve a batch at once, or, uses multiple C++ factors.
         landmarks: list of 3d points
         measured: concatenated px measurements
         TODO: flatten landmarks"""
-        noise = noiseModel.Diagonal.Sigmas(np.concatenate([[1, 1] for _ in landmarks]))
-        self._new_factors.push_back(
-            apriltag_smooth_batch.factor(
-                landmarks, measured, camera_offset, calib, noise, X(t0_us)
+        if USE_BATCH:
+            noise = noiseModel.Diagonal.Sigmas(
+                np.concatenate([[1, 1] for _ in landmarks])
             )
-        )
+            self._new_factors.push_back(
+                apriltag_smooth_batch.factor(
+                    landmarks, measured, camera_offset, calib, noise, X(t0_us)
+                )
+            )
+        else:
+            for i, landmark in enumerate(landmarks):
+                px = measured[i * 2 : (i + 1) * 2]
+                noise = noiseModel.Diagonal.Sigmas(np.array([1, 1]))
+                self._new_factors.push_back(
+                    apriltag_smooth.factor(
+                        landmark, px, camera_offset, calib, noise, X(t0_us)
+                    )
+                )
 
     def update(self) -> None:
         """Run the solver"""

raspberry_pi/app/pose_estimator/factors/apriltag_calibrate.py

@@ -34,7 +34,7 @@ def h_fn(
     landmark: np.ndarray,
 ) -> Callable[[gtsam.Pose2, gtsam.Pose3, gtsam.Cal3DS2], np.ndarray]:
     """Returns a pixel estimation function for a constant landmark,
-    with variable pose, offset, and calibration. 
+    with variable pose, offset, and calibration.
     The landmark is the field position of a tag corner."""
 
     def h(p0: gtsam.Pose2, offset: gtsam.Pose3, calib: gtsam.Cal3DS2) -> np.ndarray:
@@ -43,6 +43,8 @@ def h(p0: gtsam.Pose2, offset: gtsam.Pose3, calib: gtsam.Cal3DS2) -> np.ndarray:
         offset_pose = gtsam.Pose3(p0).compose(offset)
         # this is z-forward y-down
         camera_pose = offset_pose.compose(CAM_COORD)
+        print("camera_pose", camera_pose)
+        print("landmark", landmark)
         camera = gtsam.PinholeCameraCal3DS2(camera_pose, calib)
         return camera.project(landmark)
 
@@ -67,7 +69,7 @@ def h_H(
     return result
 
 
-def factor(
+def factorCustom(
     landmark: np.ndarray,
     measured: np.ndarray,
     model: SharedNoiseModel,
@@ -95,3 +97,28 @@ def error_func(
         gtsam.KeyVector([p0_key, offset_key, calib_key]),  # type:ignore
         error_func,
     )
+
+
+def factorNative(
+    landmark: np.ndarray,
+    measured: np.ndarray,
+    model: SharedNoiseModel,
+    p0_key: int,
+    offset_key: int,
+    calib_key: int,
+) -> gtsam.NoiseModelFactor:
+    return gtsam.PlanarSFMFactor(
+        landmark, measured, model, p0_key, offset_key, calib_key
+    )
+
+
+def factor(
+    landmark: np.ndarray,
+    measured: np.ndarray,
+    model: SharedNoiseModel,
+    p0_key: int,
+    offset_key: int,
+    calib_key: int,
+) -> gtsam.NoiseModelFactor:
+    return factorNative(landmark, measured, model, p0_key, offset_key, calib_key)
+    # return factorCustom(landmark, measured, model, p0_key, offset_key, calib_key)

raspberry_pi/app/pose_estimator/factors/apriltag_smooth.py

@@ -69,7 +69,7 @@ def h_H(
     return result
 
 
-def factor(
+def factorCustom(
     landmark: np.ndarray,
     measured: np.ndarray,
     offset: gtsam.Pose3,
@@ -93,3 +93,28 @@ def error_func(
         gtsam.KeyVector([p0_key]),  # type:ignore
         error_func,
     )
+
+
+def factorNative(
+    landmark: np.ndarray,
+    measured: np.ndarray,
+    offset: gtsam.Pose3,
+    calib: gtsam.Cal3DS2,
+    model: SharedNoiseModel,
+    p0_key: int,
+) -> gtsam.NoiseModelFactor:
+    return gtsam.PlanarProjectionFactor(
+        landmark, measured, offset, calib, model, p0_key
+    )
+
+
+def factor(
+    landmark: np.ndarray,
+    measured: np.ndarray,
+    offset: gtsam.Pose3,
+    calib: gtsam.Cal3DS2,
+    model: SharedNoiseModel,
+    p0_key: int,
+) -> gtsam.NoiseModelFactor:
+    return factorNative(landmark, measured, offset, calib, model, p0_key)
+    # return factorCustom(landmark, measured, offset, calib, model, p0_key)

raspberry_pi/app/pose_estimator/nt_estimate.py

@@ -37,7 +37,7 @@ def __init__(self, field_map: FieldMap, cam: CameraConfig, net: Network) -> None
         self.gyro_receiver = net.get_gyro_receiver("gyro")
         self.pose_sender = net.get_pose_sender("pose")
 
-        self.est = Estimate()
+        self.est = Estimate(0.1)
         # current estimate, used for initial value for next time
         # TODO: remove gtsam types
         self.state = gtsam.Pose2()
@@ -98,7 +98,7 @@ def _receive_prior(self) -> None:
         # TODO: supply sigma on the wire?
         s = noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.05]))
 
-        self.est = Estimate()
+        self.est = Estimate(0.1)
         self.state = p
         self.est.init()
         now = self.net.now()