Sim fixes

src/main/kotlin/com/team4099/lib/math/GeomUtil.kt

@@ -4,8 +4,10 @@ import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Transform2d
 import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.geometry.Twist2d
+import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.derived.radians
 
 /**
@@ -47,3 +49,11 @@ fun Pose2d.asTransform2d(): Transform2d {
 fun Transform2d.asPose2d(): Pose2d {
   return Pose2d(this.translation, this.rotation)
 }
+
+fun Transform2d.asDoubleArray(): DoubleArray {
+  return doubleArrayOf(this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians)
+}
+
+fun Pose2d.asDoubleArray(): DoubleArray {
+  return doubleArrayOf(this.x.inMeters, this.y.inMeters, this.rotation.inRadians)
+}

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -5,6 +5,7 @@ import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
@@ -32,7 +33,6 @@ import com.team4099.robot2023.subsystems.vision.camera.CameraIOSim
 import com.team4099.robot2023.subsystems.wrist.Wrist
 import com.team4099.robot2023.subsystems.wrist.WristIO
 import com.team4099.robot2023.subsystems.wrist.WristIOSim
-import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision
 import com.team4099.robot2023.util.driver.Ryan
 import edu.wpi.first.wpilibj.RobotBase
 import org.team4099.lib.smoothDeadband
@@ -61,9 +61,9 @@ object RobotContainer {
         Vision(
           //          object: CameraIO {}
           //          CameraIONorthstar("northstar"),
-          CameraIOPhotonvision("parakeet_1"),
-          CameraIOPhotonvision("parakeet_2"),
-          CameraIOPhotonvision("parakeet_3"),
+          CameraIOPhotonvision("parakeet_1", VisionConstants.CAMERA_TRANSFORMS[0]),
+          CameraIOPhotonvision("parakeet_2", VisionConstants.CAMERA_TRANSFORMS[1]),
+          CameraIOPhotonvision("parakeet_3", VisionConstants.CAMERA_TRANSFORMS[2]),
           //        CameraIONorthstar("right"),
           //        CameraIONorthstar("backward")
         )
@@ -76,7 +76,7 @@ object RobotContainer {
     } else {
       // Simulation implementations
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
-      vision = Vision(CameraIOSim("skrt"))
+      vision = Vision(CameraIOSim("skrt", VisionConstants.SIM_CAMERA_TRANSFORM))
       limelight = LimelightVision(object : LimelightVisionIO {})
       intake = Intake(IntakeIOSim)
       feeder = Feeder(FeederIOSim)

src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt

@@ -56,6 +56,11 @@ object VisionConstants {
       ) // camera facing leftward
     )
 
+  val SIM_CAMERA_TRANSFORM = Transform3d(
+    Translation3d(0.inches, 0.inches, 0.inches),
+    Rotation3d(0.degrees, 0.degrees, 0.degrees)
+  )
+
   val CAMERA_NAMES = listOf("northstar_1", "northstar_2", "northstar_3")
 
   object Limelight {

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt

@@ -297,7 +297,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
         .pose3d
     )
 
-    Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d)
+    Logger.recordOutput("FieldFrameEstimator/odomTField", doubleArrayOf(odomTField.translation.x.inMeters, odomTField.translation.y.inMeters, odomTField.rotation.inRadians))
 
     Logger.recordOutput(
       "Odometry/targetPose",
@@ -389,7 +389,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
       undriftedSwerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules)
 
       drift = undriftedPose.minus(odomTRobot)
-      Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d)
+      Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, doubleArrayOf(undriftedPose.x.inMeters, undriftedPose.y.inMeters, undriftedPose.rotation.inRadians))
     }
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt

@@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.vision
 
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.TunableNumber
+import com.team4099.lib.math.asDoubleArray
 import com.team4099.lib.vision.TimestampedVisionUpdate
 import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.config.constants.VisionConstants
@@ -14,24 +15,27 @@ import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Pose2dWPILIB
 import org.team4099.lib.geometry.Pose3d
 import org.team4099.lib.geometry.Pose3dWPILIB
+import org.team4099.lib.geometry.Transform3d
 import org.team4099.lib.units.base.Time
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.inMilliseconds
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRadians
 import java.util.function.Consumer
 import java.util.function.Supplier
 import kotlin.math.pow
 
 class Vision(vararg cameras: CameraIO) : SubsystemBase() {
   val io: List<CameraIO> = cameras.toList()
   val inputs = List(io.size) { CameraIO.CameraInputs() }
+  val names = mutableListOf<String>()
+  val robotTCameras = mutableListOf<Transform3d>()
 
   companion object {
     val ambiguityThreshold = 0.7
     val targetLogTime = 0.05.seconds
-    val cameraPoses = VisionConstants.CAMERA_TRANSFORMS
 
     val xyStdDevCoeffecient = 0.05
     val thetaStdDevCoefficient = 1.5
@@ -49,6 +53,9 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
   init {
     for (i in io.indices) {
       lastFrameTimes[i] = 0.0.seconds
+      names.add(io[i].id)
+      robotTCameras.add(io[i].robotTCamera)
+
     }
   }
 
@@ -75,7 +82,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
 
     for (instance in io.indices) {
       io[instance].updateInputs(inputs[instance])
-      Logger.processInputs("Vision/${VisionConstants.CAMERA_NAMES[instance]}", inputs[instance])
+      Logger.processInputs("Vision/${names[instance]}", inputs[instance])
     }
 
     var fieldTCurrentRobotEstimate: Pose2d = fieldFramePoseSupplier.get()
@@ -90,7 +97,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
       val values = inputs[instance].frame
 
       var cameraPose: Pose3d? = inputs[instance].frame
-      var robotPose: Pose2d? = cameraPose?.transformBy(cameraPoses[instance])?.toPose2d()
+      var robotPose: Pose2d? = cameraPose?.transformBy(robotTCameras[instance])?.toPose2d()
 
 
       if (cameraPose == null || robotPose == null) {
@@ -125,30 +132,30 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
       robotPoses.add(robotPose)
 
       Logger.recordOutput(
-        "Vision/${VisionConstants.CAMERA_NAMES[instance]}/latencyMS",
+        "Vision/${names[instance]}/latencyMS",
         (Clock.fpgaTime - timestamp).inMilliseconds
       )
 
       Logger.recordOutput(
-        "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", robotPose.pose2d
+        "Vision/${names[instance]}/estimatedRobotPose", robotPose.asDoubleArray()
       )
 
       Logger.recordOutput(
-        "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses",
+        "Vision/${names[instance]}/tagPoses",
         *tagPoses.map { it.pose3d }.toTypedArray()
       )
 
-      if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol
-        Logger.recordOutput(
-          "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose",
-          Pose2dWPILIB.struct,
-          Pose2d().pose2d
-        )
-      }
+//      if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol
+//        Logger.recordOutput(
+//          "Vision/${names[instance]}/estimatedRobotPose",
+//          Pose2dWPILIB.struct,
+//          Pose2d().pose2d
+//        )
+//      }
 
       if (Clock.fpgaTime - lastFrameTimes[instance]!! > targetLogTime) {
         Logger.recordOutput(
-          "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses",
+          "Vision/${names[instance]}/tagPoses",
           Pose3dWPILIB.struct,
           *arrayOf<Pose3dWPILIB>()
         )

src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt

@@ -5,10 +5,14 @@ import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
 import org.team4099.lib.geometry.Pose3d
 import org.team4099.lib.geometry.Pose3dWPILIB
+import org.team4099.lib.geometry.Transform3d
 import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.seconds
 
 interface CameraIO {
+
+  val id: String
+  val robotTCamera: Transform3d
   class CameraInputs : LoggableInputs {
     var timestamp = 0.0.seconds
     var frame: Pose3d = Pose3d()

src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt

@@ -6,21 +6,22 @@ import org.photonvision.PhotonCamera
 import org.photonvision.PhotonPoseEstimator
 import org.photonvision.PhotonPoseEstimator.PoseStrategy
 import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Transform3d
 import org.team4099.lib.geometry.Transform3dWPILIB
 import org.team4099.lib.units.base.Time
 import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.micro
 import java.util.Optional
 
-class CameraIOPhotonvision(private val identifier: String) : CameraIO {
+class CameraIOPhotonvision(override val id: String, override val robotTCamera: Transform3d) : CameraIO {
 
   private val photonEstimator: PhotonPoseEstimator
   private val camera: PhotonCamera
   private var lastEstTimestamp: Time = 0.0.seconds
 
   init {
-    camera = PhotonCamera(identifier)
+    camera = PhotonCamera(id)
     photonEstimator =
       PhotonPoseEstimator(
         FieldConstants.wpilibFieldLayout,

src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt

@@ -12,13 +12,14 @@ import org.littletonrobotics.junction.Logger
 import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Pose3d
 import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.geometry.Transform3d
 import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.geometry.Translation3d
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.radians
 
-class CameraIOSim(val id: String) : CameraIO {
+class CameraIOSim(override val id: String, override val robotTCamera: Transform3d) : CameraIO {
   private val poseSubscriber: DoubleArraySubscriber =
     NetworkTableInstance.getDefault()
       .getTable("AdvantageKit")
@@ -32,7 +33,7 @@ class CameraIOSim(val id: String) : CameraIO {
 
     inputs.timestamp = Clock.fpgaTime - (Math.random() * 0.2).seconds
     val robotTtag = drivePose.relativeTo(FieldConstants.aprilTags[0].pose)
-    inputs.frame = robotTtag.gaussianShenanigans()
+    inputs.frame = drivePose.gaussianShenanigans()
     val estimatedRobotPose =
       FieldConstants.aprilTags[0].pose.transformBy(inputs.frame.toTransform3d())