From 751a49d72c0c7ba086101bd07c67f1d6382ec6ea Mon Sep 17 00:00:00 2001 From: Parth Oza <3963740+plusparth@users.noreply.github.com> Date: Sat, 17 Feb 2024 15:25:14 -0800 Subject: [PATCH 1/3] Add field frame estimator (#18) Estimates the transform between the odometry frame and the field frame, given drive poses in the odometry frame and vision poses in the field frame. Topic: field-frame-estimator Reviewers: saraansh, rithvik --- .../kotlin/com/team4099/lib/math/GeomUtil.kt | 12 + .../robot2023/util/FieldFrameEstimator.kt | 223 ++++++++++++++++++ 2 files changed, 235 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt diff --git a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt index 36b1897b..7fc92d99 100644 --- a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt +++ b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt @@ -4,7 +4,9 @@ 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.meters import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.radians /** * Multiplies a twist by a scaling factor @@ -27,3 +29,13 @@ fun multiplyTwist(twist: Twist2d, factor: Double): Twist2d { fun Pose2d.purelyTranslateBy(translation2d: Translation2d): Pose2d { return this.transformBy(Transform2d(translation2d.rotateBy(-this.rotation), 0.0.degrees)) } + +/** + * Returns the transform between the frame origin of the pose and the current pose state -- for + * example, if the pose describes the pose of the robot in the odometry frame, the returned + * transform will be the transform between the odometry frame and the robot frame. + * @return + */ +fun Pose2d.asTransform2d(): Transform2d { + return Transform2d(Pose2d(0.meters, 0.meters, 0.radians), this) +} diff --git a/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt b/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt new file mode 100644 index 00000000..b4a4955e --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt @@ -0,0 +1,223 @@ +package com.team4099.robot2023.util + +import com.team4099.lib.hal.Clock +import com.team4099.lib.math.asTransform2d +import edu.wpi.first.math.Matrix +import edu.wpi.first.math.Nat +import edu.wpi.first.math.VecBuilder +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import org.littletonrobotics.junction.Logger +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Pose2dWPILIB +import org.team4099.lib.geometry.Transform2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.geometry.Twist2d +import org.team4099.lib.units.base.Time +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.radians +import java.util.NavigableMap +import java.util.TreeMap +import kotlin.Comparator +import kotlin.collections.ArrayList + +class FieldFrameEstimator(stateStdDevs: Matrix) { + // Maintains the state of the field frame transform for the period of time before the currently + // tracked history + private var baseOdometryTField: Transform2d = + Transform2d(Translation2d(0.meters, 0.meters), 0.radians) + + // Maintains the latest state of the field frame transform, including the currently tracked + // history + private var odometryTField: Transform2d = + Transform2d(Translation2d(0.meters, 0.meters), 0.radians) + + private val updates: NavigableMap = TreeMap() + private val q: Matrix = Matrix(Nat.N3(), Nat.N1()) + + /** Returns the latest robot pose based on drive and vision data. */ + fun getLatestOdometryTField(): Transform2d { + return odometryTField + } + + /** Resets the field frame transform to a known pose. */ + fun resetFieldFrameFilter(transform: Transform2d) { + baseOdometryTField = transform + updates.clear() + update() + } + + /** Records a new drive movement. */ + fun addDriveData(timestamp: Time, odomTRobot: Pose2d) { + updates[timestamp] = PoseUpdate(odomTRobot, ArrayList()) + update() + } + + /** Records a new set of vision updates. */ + fun addVisionData(visionData: List) { + for (timestampedVisionUpdate in visionData) { + val timestamp: Time = timestampedVisionUpdate.timestamp + val visionUpdate = + VisionUpdate( + timestampedVisionUpdate.fieldTRobot, + timestampedVisionUpdate.stdDevs, + timestampedVisionUpdate.fromVision + ) + if (updates.containsKey(timestamp)) { + // There was already an update at this timestamp, add to it + val oldVisionUpdates: ArrayList = updates[timestamp]!!.visionUpdates + oldVisionUpdates.add(visionUpdate) + oldVisionUpdates.sortWith(VisionUpdate.compareDescStdDev) + } else { + // Insert a new update + val prevUpdate = updates.floorEntry(timestamp) + val nextUpdate = updates.ceilingEntry(timestamp) + if (prevUpdate == null || nextUpdate == null) { + // Outside the range of existing data + return + } + + // Create partial twists (prev -> vision, vision -> next) + val prevToVisionTwist = + multiplyTwist( + prevUpdate.value.odomTRobot.log(nextUpdate.value.odomTRobot), + (timestamp - prevUpdate.key) / (nextUpdate.key - prevUpdate.key) + ) + + // Add new pose updates + val newVisionUpdates = ArrayList() + newVisionUpdates.add(visionUpdate) + newVisionUpdates.sortWith(VisionUpdate.compareDescStdDev) + updates[timestamp] = + PoseUpdate(prevUpdate.value.odomTRobot.exp(prevToVisionTwist), newVisionUpdates) + } + } + + // Recalculate latest pose once + update() + } + + /** Clears old data and calculates the latest pose. */ + private fun update() { + // Clear old data and update base pose + // NOTE(parth): We need to maintain the history so that when vision updates come in, they have + // some buffer to interpolate within. + while (updates.size > 1 && updates.firstKey() < Clock.fpgaTime - HISTORY_LENGTH) { + val (_, value) = updates.pollFirstEntry() + baseOdometryTField = value.apply(baseOdometryTField, q) + } + + // Update latest pose + odometryTField = baseOdometryTField + for (updateEntry in updates.entries) { + odometryTField = updateEntry.value.apply(odometryTField, q) + } + + for (update in updates) { + if (update.value.visionUpdates.size > 0 && update.value.visionUpdates[0].fromVision) { + Logger.recordOutput("Vision/Buffer/Vision", update.key.inSeconds) + + Logger.recordOutput( + "Vision/Buffer/VisionPose", + Pose2dWPILIB.struct, + update.value.visionUpdates[0].fieldTRobot.pose2d + ) + } else { + Logger.recordOutput("Vision/Buffer/Drivetrain", update.key.inSeconds) + } + } + } + + /** + * Represents a sequential update to a pose estimate, with a twist (drive movement) and list of + * vision updates. + */ + private class PoseUpdate(val odomTRobot: Pose2d, val visionUpdates: ArrayList) { + fun apply(previousOdomTField: Transform2d, q: Matrix): Transform2d { + var currentOdomTField = previousOdomTField + + // Apply vision updates + for (visionUpdate in visionUpdates) { + // Calculate Kalman gains based on std devs + // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/) + val visionK: Matrix = Matrix(Nat.N3(), Nat.N3()) + val r = DoubleArray(3) + for (i in 0..2) { + r[i] = visionUpdate.stdDevs.get(i, 0) * visionUpdate.stdDevs.get(i, 0) + } + for (row in 0..2) { + if (q.get(row, 0) === 0.0) { + visionK.set(row, row, 0.0) + } else { + visionK.set( + row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row])) + ) + } + } + + // Calculate odom_T_field from this update's vision pose + val odomTVisionField = + odomTRobot.asTransform2d() + visionUpdate.fieldTRobot.asTransform2d().inverse() + + // Calculate twist between current field frame transform and latest vision update + val fieldTVisionField = currentOdomTField.inverse() + odomTVisionField + val visionTwist = fieldTVisionField.log() + + // Multiply by Kalman gain matrix + val twistMatrix = + visionK.times( + VecBuilder.fill( + visionTwist.dx.inMeters, visionTwist.dy.inMeters, visionTwist.dtheta.inRadians + ) + ) + + // Apply twist + currentOdomTField += + Transform2d.exp( + Twist2d( + twistMatrix.get(0, 0).meters, + twistMatrix.get(1, 0).meters, + twistMatrix.get(2, 0).radians + ) + ) + } + return currentOdomTField + } + } + + /** Represents a single vision pose with associated standard deviations. */ + class VisionUpdate( + val fieldTRobot: Pose2d, + val stdDevs: Matrix, + val fromVision: Boolean = false + ) { + companion object { + val compareDescStdDev = Comparator { a: VisionUpdate, b: VisionUpdate -> + -(a.stdDevs.get(0, 0) + a.stdDevs.get(1, 0)).compareTo( + b.stdDevs.get(0, 0) + b.stdDevs.get(1, 0) + ) + } + } + } + + /** Represents a single vision pose with a timestamp and associated standard deviations. */ + class TimestampedVisionUpdate( + val timestamp: Time, + val fieldTRobot: Pose2d, + val stdDevs: Matrix, + val fromVision: Boolean = false + ) + companion object { + private val HISTORY_LENGTH = 0.3.seconds + } + + init { + for (i in 0..2) { + q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)) + } + } +} From 86087383db9f3c0db4b98b30fa7666fcd6e012be Mon Sep 17 00:00:00 2001 From: Parth Oza Date: Sat, 17 Feb 2024 13:21:35 -0800 Subject: [PATCH 2/3] Pull TimestampedVisionUpdate out of pose estimator Topic: refactor-vision-update Relative: field-frame-estimator Reviewers: saraansh, rithvik, pranav --- .../lib/vision/TimestampedVisionUpdate.kt | 15 ++++++++++++ .../subsystems/limelight/LimelightVision.kt | 6 ++--- .../robot2023/subsystems/vision/Vision.kt | 24 +++++++++---------- .../robot2023/util/FieldFrameEstimator.kt | 8 +------ .../team4099/robot2023/util/PoseEstimator.kt | 11 ++------- 5 files changed, 33 insertions(+), 31 deletions(-) create mode 100644 src/main/kotlin/com/team4099/lib/vision/TimestampedVisionUpdate.kt diff --git a/src/main/kotlin/com/team4099/lib/vision/TimestampedVisionUpdate.kt b/src/main/kotlin/com/team4099/lib/vision/TimestampedVisionUpdate.kt new file mode 100644 index 00000000..9cccdc80 --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/vision/TimestampedVisionUpdate.kt @@ -0,0 +1,15 @@ +package com.team4099.lib.vision + +import edu.wpi.first.math.Matrix +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.units.base.Time + +/** Represents a single vision pose with a timestamp and associated standard deviations. */ +data class TimestampedVisionUpdate( + val timestamp: Time, + val fieldTRobot: Pose2d, + val stdDevs: Matrix, + val fromVision: Boolean = false +) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index 4a38b1e0..21379379 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -2,9 +2,9 @@ package com.team4099.robot2023.subsystems.limelight import com.team4099.lib.logging.TunableNumber import com.team4099.lib.vision.TargetCorner +import com.team4099.lib.vision.TimestampedVisionUpdate import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.util.LimelightReading -import com.team4099.robot2023.util.PoseEstimator import com.team4099.robot2023.util.rotateBy import com.team4099.robot2023.util.toPose3d import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -33,7 +33,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { val inputs = LimelightVisionIO.LimelightVisionIOInputs() var poseSupplier: () -> Pose2d = { Pose2d() } - var visionConsumer: Consumer> = Consumer {} + var visionConsumer: Consumer> = Consumer {} // i think we need this for camera project to irl coordinates val vpw = (2.0 * (VisionConstants.Limelight.HORIZONTAL_FOV / 2).tan) @@ -217,7 +217,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { fun setDataInterfaces( poseSupplier: () -> Pose2d, - visionConsumer: Consumer> + visionConsumer: Consumer> ) { this.poseSupplier = poseSupplier this.visionConsumer = visionConsumer diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index 1eec2086..c3ee809f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -2,9 +2,9 @@ package com.team4099.robot2023.subsystems.vision import com.team4099.lib.hal.Clock import com.team4099.lib.logging.TunableNumber +import com.team4099.lib.vision.TimestampedVisionUpdate import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.vision.camera.CameraIO -import com.team4099.robot2023.util.PoseEstimator import edu.wpi.first.math.VecBuilder import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -43,8 +43,8 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { private val thetaStdDev = TunableNumber("Vision/thetaStdDev", thetaStdDevCoefficient) - private var poseSupplier = Supplier { Pose2d() } - private var visionConsumer: Consumer> = Consumer {} + private var fieldFramePoseSupplier = Supplier { Pose2d() } + private var visionConsumer: Consumer> = Consumer {} private val lastFrameTimes = mutableMapOf() private val lastTagDetectionTimes = mutableMapOf() @@ -55,10 +55,10 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { } fun setDataInterfaces( - poseSupplier: Supplier, - visionConsumer: Consumer> + fieldFramePoseSupplier: Supplier, + visionConsumer: Consumer> ) { - this.poseSupplier = poseSupplier + this.fieldFramePoseSupplier = fieldFramePoseSupplier this.visionConsumer = visionConsumer } @@ -80,9 +80,9 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { Logger.processInputs("Vision/${VisionConstants.CAMERA_NAMES[instance]}", inputs[instance]) } - var currentPose: Pose2d = poseSupplier.get() + var fieldTCurrentRobotEstimate: Pose2d = fieldFramePoseSupplier.get() val robotPoses = mutableListOf() - val visionUpdates = mutableListOf() + val visionUpdates = mutableListOf() for (instance in io.indices) { @@ -172,8 +172,8 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val robotPose1 = cameraPose1.transformBy(cameraPoses[instance].inverse()).toPose2d() - if (robotPose0.rotation.minus(currentPose.rotation).absoluteValue < - robotPose1.rotation.minus(currentPose.rotation).absoluteValue + if (robotPose0.rotation.minus(fieldTCurrentRobotEstimate.rotation).absoluteValue < + robotPose1.rotation.minus(fieldTCurrentRobotEstimate.rotation).absoluteValue ) { cameraPose = cameraPose0 robotPose = robotPose0 @@ -193,7 +193,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { continue } - if ((robotPose.rotation - currentPose.rotation).absoluteValue > 7.degrees && + if ((robotPose.rotation - fieldTCurrentRobotEstimate.rotation).absoluteValue > 7.degrees && DriverStation.isEnabled() ) { continue @@ -219,7 +219,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val thetaStdDev = thetaStdDev.get() * averageDistance.inMeters.pow(2) / tagPoses.size visionUpdates.add( - PoseEstimator.TimestampedVisionUpdate( + TimestampedVisionUpdate( timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) ) ) diff --git a/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt b/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt index b4a4955e..a6697f37 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.util import com.team4099.lib.hal.Clock import com.team4099.lib.math.asTransform2d +import com.team4099.lib.vision.TimestampedVisionUpdate import edu.wpi.first.math.Matrix import edu.wpi.first.math.Nat import edu.wpi.first.math.VecBuilder @@ -204,13 +205,6 @@ class FieldFrameEstimator(stateStdDevs: Matrix) { } } - /** Represents a single vision pose with a timestamp and associated standard deviations. */ - class TimestampedVisionUpdate( - val timestamp: Time, - val fieldTRobot: Pose2d, - val stdDevs: Matrix, - val fromVision: Boolean = false - ) companion object { private val HISTORY_LENGTH = 0.3.seconds } diff --git a/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt b/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt index fb840cfa..16a00a7c 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.util +import com.team4099.lib.vision.TimestampedVisionUpdate import edu.wpi.first.math.Matrix import edu.wpi.first.math.Nat import edu.wpi.first.math.VecBuilder @@ -10,7 +11,6 @@ import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Twist2d -import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.meters @@ -49,7 +49,7 @@ class PoseEstimator(stateStdDevs: Matrix) { val timestamp: Double = timestampedVisionUpdate.timestamp.inSeconds val visionUpdate = VisionUpdate( - timestampedVisionUpdate.pose, + timestampedVisionUpdate.fieldTRobot, timestampedVisionUpdate.stdDevs, timestampedVisionUpdate.fromVision ) @@ -189,13 +189,6 @@ class PoseEstimator(stateStdDevs: Matrix) { } } - /** Represents a single vision pose with a timestamp and associated standard deviations. */ - class TimestampedVisionUpdate( - val timestamp: Time, - val pose: Pose2d, - val stdDevs: Matrix, - val fromVision: Boolean = false - ) companion object { private const val historyLengthSecs = 0.3 } From cefec66110f6f4c5daad35a38c6157a855aa586f Mon Sep 17 00:00:00 2001 From: Parth Oza Date: Sat, 17 Feb 2024 13:23:11 -0800 Subject: [PATCH 3/3] Use field frame estimator in drivetrain Topic: field-frame-drivetrain-2 Relative: refactor-vision-update Reviewers: saraansh, rithvik, pranav --- .../kotlin/com/team4099/lib/math/GeomUtil.kt | 8 + .../com/team4099/robot2023/RobotContainer.kt | 4 +- .../commands/drivetrain/DrivePathCommand.kt | 5 +- .../commands/drivetrain/ResetPoseCommand.kt | 2 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 205 ++++++++---------- 5 files changed, 100 insertions(+), 124 deletions(-) diff --git a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt index 7fc92d99..c3de20af 100644 --- a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt +++ b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt @@ -39,3 +39,11 @@ fun Pose2d.purelyTranslateBy(translation2d: Translation2d): Pose2d { fun Pose2d.asTransform2d(): Transform2d { return Transform2d(Pose2d(0.meters, 0.meters, 0.radians), this) } + +/** + * Returns pose of whatever the transform represents, in the original frame of the transform. For + * example, odomTRobot.asPose2d() would give the pose of the robot in the odometry frame. + */ +fun Transform2d.asPose2d(): Pose2d { + return Pose2d(this.translation, this.rotation) +} diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 9f8de0aa..7269090f 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -89,8 +89,8 @@ object RobotContainer { } superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel) - vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) - limelight.poseSupplier = { drivetrain.odometryPose } + vision.setDataInterfaces({ drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) }) + limelight.poseSupplier = { drivetrain.odomTRobot } } fun mapDefaultCommands() { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 454a69be..ed604241 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -1,7 +1,6 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.lib.pathfollow.Trajectory import com.team4099.lib.trajectory.CustomHolonomicDriveController import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.lib.trajectory.Waypoint @@ -227,9 +226,7 @@ class DrivePathCommand( desiredState.curvatureRadPerMeter.radians.sin var nextDriveState = - swerveDriveController.calculate( - drivetrain.odometryPose.pose2d, desiredState, desiredRotation - ) + swerveDriveController.calculate(drivetrain.odomTRobot.pose2d, desiredState, desiredRotation) if (leaveOutYAdjustment) { nextDriveState = diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt index 305c8f00..96c58b6f 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt @@ -13,7 +13,7 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() } override fun initialize() { - drivetrain.odometryPose = AllianceFlipUtil.apply(pose) + drivetrain.resetFieldFrameEstimator(AllianceFlipUtil.apply(pose)) Logger.recordOutput( "Drivetrain/lastResetPose", Pose2dWPILIB.struct, AllianceFlipUtil.apply(pose).pose2d ) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 7c2f28c2..2906c145 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -2,6 +2,9 @@ package com.team4099.robot2023.subsystems.drivetrain.drive import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.lib.math.asPose2d +import com.team4099.lib.math.asTransform2d +import com.team4099.lib.vision.TimestampedVisionUpdate import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.config.constants.VisionConstants @@ -9,7 +12,7 @@ import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData -import com.team4099.robot2023.util.PoseEstimator +import com.team4099.robot2023.util.FieldFrameEstimator import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.VecBuilder import edu.wpi.first.math.kinematics.SwerveDriveKinematics @@ -33,7 +36,6 @@ import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds -import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -48,42 +50,45 @@ import java.util.concurrent.locks.ReentrantLock import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { - val gyroNotConnectedAlert = + private val gyroNotConnectedAlert = Alert( "Gyro is not connected, field relative driving will be significantly worse.", Alert.AlertType.ERROR ) val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR - val gyroInputs = GyroIO.GyroIOInputs() + private val gyroInputs = GyroIO.GyroIOInputs() - var gyroYawOffset = 0.0.radians + private var gyroYawOffset = 0.0.radians - var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.0001)) + // TODO: tune these + private var fieldFrameEstimator = FieldFrameEstimator(VecBuilder.fill(0.003, 0.003, 0.0001)) - var angularVelocityTarget = 0.degrees.perSecond + private var angularVelocityTarget = 0.degrees.perSecond - var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) + private var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) - var isFieldOrientated = true // true denotes that when driving, it'll be field oriented. + private var isFieldOriented = true - var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + private var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + private var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) - var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) + private var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + private set - var robotVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + private var robotVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) - var omegaVelocity = 0.0.radians.perSecond + private var omegaVelocity = 0.0.radians.perSecond var lastGyroYaw = 0.0.radians var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED + private set private val testAngle = LoggedTunableValue("Drivetrain/testAngle", 0.degrees, Pair({ it.inDegrees }, { it.degrees })) @@ -94,7 +99,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB val closestAlignmentAngle: Angle get() { for (angle in -180..90 step 90) { - if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { + if ((odomTRobot.rotation - angle.degrees).absoluteValue <= 45.degrees) { return angle.degrees } } @@ -107,7 +112,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB is DrivetrainRequest.OpenLoop -> { angularVelocityTarget = value.angularVelocity targetedDriveVector = value.driveVector - isFieldOrientated = value.fieldOriented + isFieldOriented = value.fieldOriented } is DrivetrainRequest.ClosedLoop -> { targetedChassisSpeeds = value.chassisSpeeds @@ -143,7 +148,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB backRightWheelLocation ) - val swerveDriveKinematics = + private val swerveDriveKinematics = SwerveDriveKinematics( frontLeftWheelLocation.translation2d, frontRightWheelLocation.translation2d, @@ -151,43 +156,51 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB backRightWheelLocation.translation2d ) - var swerveDriveOdometry = + private var swerveDriveOdometry = SwerveDriveOdometry( swerveDriveKinematics, gyroInputs.gyroYaw.inRotation2ds, swerveModules.map { it.modulePosition }.toTypedArray() ) - var setPointStates = + private var undriftedSwerveDriveOdometry = + SwerveDriveOdometry( + swerveDriveKinematics, + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray() + ) + + private var setPointStates = mutableListOf( SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() ) - var odometryPose: Pose2d - get() = swerveDrivePoseEstimator.getLatestPose() - set(value) { - swerveDrivePoseEstimator.resetPose(value) + val odomTRobot: Pose2d + get() = Pose2d(swerveDriveOdometry.poseMeters) - if (RobotBase.isReal()) {} else { - undriftedPose = odometryPose - swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 - } - } + // NOTE(parth): This should be expected to be noisy. Avoid using this directly if possible. + val fieldTRobot: Pose2d + get() = + (fieldFrameEstimator.getLatestOdometryTField().inverse() + odomTRobot.asTransform2d()) + .asPose2d() - var undriftedPose: Pose2d - get() = Pose2d(swerveDriveOdometry.poseMeters) + val odomTField: Transform2d + get() = fieldFrameEstimator.getLatestOdometryTField() + + private var undriftedPose: Pose2d + get() = Pose2d(undriftedSwerveDriveOdometry.poseMeters) set(value) { - swerveDriveOdometry.resetPosition( + undriftedSwerveDriveOdometry.resetPosition( gyroInputs.gyroYaw.inRotation2ds, swerveModules.map { it.modulePosition }.toTypedArray(), value.pose2d ) } - var rawGyroAngle = odometryPose.rotation + private var rawGyroAngle = odomTRobot.rotation - val lastModulePositions = - mutableListOf( + private val lastModulePositions = + arrayOf( SwerveModulePosition(), SwerveModulePosition(), SwerveModulePosition(), @@ -230,7 +243,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Translation2d( chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters ) - .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig + .rotateBy(odomTRobot.rotation) // we don't use this but it's there if you want it ig robotVelocity = Velocity2d(chassisState.vx, chassisState.vy) fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) @@ -245,7 +258,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB // updating odometry every loop cycle updateOdometry() - Logger.recordOutput("Drivetrain/OdometryGyroRotationValue", odometryPose.rotation.inDegrees) + Logger.recordOutput("Drivetrain/OdometryGyroRotationValue", odomTRobot.rotation.inDegrees) Logger.recordOutput( "Drivetrain/xRobotVelocityMetersPerSecond", robotVelocity.x.inMetersPerSecond @@ -263,9 +276,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Logger.processInputs("Drivetrain/Gyro", gyroInputs) Logger.recordOutput( VisionConstants.POSE_TOPIC_NAME, - doubleArrayOf( - odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians - ) + doubleArrayOf(odomTRobot.x.inMeters, odomTRobot.y.inMeters, odomTRobot.rotation.inRadians) ) Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) @@ -273,21 +284,21 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Logger.recordOutput( VisionConstants.POSE_TOPIC_NAME, - doubleArrayOf( - odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians - ) + doubleArrayOf(odomTRobot.x.inMeters, odomTRobot.y.inMeters, odomTRobot.rotation.inRadians) ) Logger.recordOutput( "Odometry/pose3d", Pose3d( - odometryPose.x, - odometryPose.y, + odomTRobot.x, + odomTRobot.y, 1.0.meters, Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) ) .pose3d ) + Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d) + Logger.recordOutput( "Odometry/targetPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) @@ -321,7 +332,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } DrivetrainState.OPEN_LOOP -> { // Outputs - setOpenLoop(angularVelocityTarget, targetedDriveVector, isFieldOrientated) + setOpenLoop(angularVelocityTarget, targetedDriveVector, isFieldOriented) Logger.recordOutput( "Drivetrain/TargetVelocityX", targetedDriveVector.first.inMetersPerSecond ) @@ -347,57 +358,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } private fun updateOdometry() { - - // // find device with min data updates and update odometry with that many deltas - // var deltaCount = - // if (gyroInputs.gyroConnected) gyroInputs.odometryYawPositions.size else - // Integer.MAX_VALUE - // for (i in 0..3) { - // deltaCount = Math.min(deltaCount, swerveModules[i].positionDeltas.size) - // } - // - // // update odometry pose for each delta - // for (deltaIndex in 0..deltaCount - 1) { - // // Read wheel deltas from each module - // val wheelDeltas = arrayOfNulls(4) - // for (moduleIndex in 0..3) { - // wheelDeltas[moduleIndex] = swerveModules[moduleIndex].positionDeltas.removeFirst() - // } - // - // // generate twist from wheel and gyro deltas - // var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas) - // if (gyroInputs.gyroConnected) { - // val currentGyroYaw = gyroInputs.rawGyroYaw - // Logger.recordOutput("Drivetrain/yeeYaw", gyroInputs.rawGyroYaw.inDegrees) - // Logger.recordOutput("Drivetrain/lastYeeYaw", lastGyroYaw.inDegrees) - // driveTwist = - // edu.wpi.first.math.geometry.Twist2d( - // driveTwist.dx, driveTwist.dy, -(currentGyroYaw - lastGyroYaw).inRadians - // ) - // lastGyroYaw = gyroInputs.rawGyroYaw - // Logger.recordOutput("Drivetrain/dxMeters", driveTwist.dx) - // Logger.recordOutput("Drivetrain/dYMeters", driveTwist.dy) - // Logger.recordOutput("Drivetrain/dThetaDegrees", -(currentGyroYaw - - // lastGyroYaw).inRadians) - // } - // - // swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) - // } - - val wheelDeltas = - mutableListOf( - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition() - ) for (i in 0 until 4) { - wheelDeltas[i] = - SwerveModulePosition( - swerveModules[i].inputs.drivePosition.inMeters - - lastModulePositions[i].distanceMeters, - swerveModules[i].inputs.steeringPosition.inRotation2ds - ) lastModulePositions[i] = SwerveModulePosition( swerveModules[i].inputs.drivePosition.inMeters, @@ -405,19 +366,13 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ) } - var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - - if (gyroInputs.gyroConnected) { - driveTwist = - edu.wpi.first.math.geometry.Twist2d( - driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians - ) - lastGyroYaw = gyroInputs.rawGyroYaw - } - - swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) + // TODO(parth): The thing I removed here may have removed support for driving field oriented w/o + // a gyro. Check before merging + // TODO(parth): For this to work our gyro rate coefficient thing needs to be correct + swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, lastModulePositions) + fieldFrameEstimator.addDriveData(Clock.fpgaTime, odomTRobot) - // reversing the drift to store the ground truth pose + // reversing the drift to store the sim ground truth pose if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { val undriftedModules = arrayOfNulls(4) for (i in 0 until 4) { @@ -431,9 +386,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB swerveModules[i].modulePosition.angle ) } - swerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules) + undriftedSwerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules) - drift = undriftedPose.minus(odometryPose) + drift = undriftedPose.minus(odomTRobot) Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) } } @@ -465,7 +420,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB allianceFlippedDriveVector.first, allianceFlippedDriveVector.second, angularVelocity, - odometryPose.rotation + odomTRobot.rotation ) } else { desiredChassisSpeeds = @@ -586,24 +541,40 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB zeroDrive() } + /** Resets the field frame estimator given some current pose of the robot. */ + fun resetFieldFrameEstimator(fieldTRobot: Pose2d) { + fieldFrameEstimator.resetFieldFrameFilter( + odomTRobot.asTransform2d() + fieldTRobot.asTransform2d().inverse() + ) + } + /** * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. * * @param toAngle Zeros the gyro to the value */ fun zeroGyroYaw(toAngle: Angle = 0.degrees) { - gyroIO.zeroGyroYaw(toAngle) - - swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) - + // TODO(parth): This feels incorrect -- I think the first arg should be the gyro angle and the + // undrifted pose should be updated to toAngle if (RobotBase.isSimulation()) { - swerveDriveOdometry.resetPosition( + // NOTE(parth): The gyro itself should never need to be reset in-match on a real robot, the + // odometry can be updated directly + gyroIO.zeroGyroYaw(toAngle) + undriftedSwerveDriveOdometry.resetPosition( toAngle.inRotation2ds, swerveModules.map { it.modulePosition }.toTypedArray(), undriftedPose.pose2d ) } + // TODO(parth): Update the field frame estimator's transform here too, otherwise it will need to + // re-converge + swerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + lastModulePositions, + Pose2d(odomTRobot.x, odomTRobot.y, toAngle).pose2d + ) + if (!(gyroInputs.gyroConnected)) { gyroYawOffset = toAngle - rawGyroAngle } @@ -635,8 +606,8 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } } - fun addVisionData(visionData: List) { - swerveDrivePoseEstimator.addVisionData(visionData) + fun addVisionData(visionData: List) { + fieldFrameEstimator.addVisionData(visionData) } fun lockWheels() {