From 39a8984ce3b107beea78031f527dff0a4a253071 Mon Sep 17 00:00:00 2001 From: Parth Oza Date: Sat, 17 Feb 2024 13:21:35 -0800 Subject: [PATCH 1/5] Pull TimestampedVisionUpdate out of pose estimator Topic: refactor-vision-update Relative: field-frame-estimator Reviewers: saraansh, rithvik, pranav --- .../lib/vision/TimestampedVisionUpdate.kt | 15 ++++++++++++ .../subsystems/drivetrain/drive/Drivetrain.kt | 3 ++- .../subsystems/limelight/LimelightVision.kt | 6 ++--- .../robot2023/subsystems/vision/Vision.kt | 24 +++++++++---------- .../robot2023/util/FieldFrameEstimator.kt | 8 +------ .../team4099/robot2023/util/PoseEstimator.kt | 11 ++------- 6 files changed, 35 insertions(+), 32 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/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 7c2f28c2..faed7283 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,7 @@ package com.team4099.robot2023.subsystems.drivetrain.drive import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +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 @@ -635,7 +636,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } } - fun addVisionData(visionData: List) { + fun addVisionData(visionData: List) { swerveDrivePoseEstimator.addVisionData(visionData) } 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 99930a9b37076d10c97ad2b3828054d7dc20d412 Mon Sep 17 00:00:00 2001 From: Parth Oza Date: Sat, 17 Feb 2024 13:23:11 -0800 Subject: [PATCH 2/5] 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 +- .../FollowPathPlannerPathCommand.kt | 10 +- .../commands/drivetrain/ResetPoseCommand.kt | 2 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 200 ++++++++---------- 6 files changed, 103 insertions(+), 126 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 b7d5cb53..0815765c 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/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt index e9c14ed4..bf58d0a0 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt @@ -53,9 +53,9 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla private val atReference: Boolean get() = ( - (lastSampledPose.translation - drivetrain.odometryPose.translation).magnitude.meters <= + (lastSampledPose.translation - drivetrain.odomTRobot.translation).magnitude.meters <= translationToleranceAtEnd && - (lastSampledPose.rotation - drivetrain.odometryPose.rotation).absoluteValue <= + (lastSampledPose.rotation - drivetrain.odomTRobot.rotation).absoluteValue <= thetaToleranceAtEnd ) @@ -176,7 +176,7 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla } val currentSpeeds = drivetrain.targetedChassisSpeeds - val poseRotation = drivetrain.odometryPose.rotation.inRotation2ds + val poseRotation = drivetrain.odomTRobot.rotation.inRotation2ds val generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation) // Sampling the trajectory for a state that we're trying to target @@ -194,13 +194,13 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla Logger.recordOutput( "Pathfollow/thetaSetpoint", stateFromTrajectory.targetHolonomicPose.rotation.degrees ) - Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odometryPose.rotation.inDegrees) + Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odomTRobot.rotation.inDegrees) lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose) val targetedChassisSpeeds = swerveDriveController.calculateRobotRelativeSpeeds( - drivetrain.odometryPose, stateFromTrajectory + drivetrain.odomTRobot, stateFromTrajectory ) // Set closed loop request 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 faed7283..8105f929 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,8 @@ 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 @@ -10,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 @@ -34,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 @@ -49,42 +50,47 @@ 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 set var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + private set 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 })) @@ -95,7 +101,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 } } @@ -108,7 +114,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 @@ -144,7 +150,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB backRightWheelLocation ) - val swerveDriveKinematics = + private val swerveDriveKinematics = SwerveDriveKinematics( frontLeftWheelLocation.translation2d, frontRightWheelLocation.translation2d, @@ -152,43 +158,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(), @@ -231,7 +245,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) @@ -246,7 +260,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 @@ -264,9 +278,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) @@ -274,21 +286,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) @@ -322,7 +334,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 ) @@ -348,57 +360,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, @@ -406,19 +368,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) { @@ -432,9 +388,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) } } @@ -466,7 +422,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB allianceFlippedDriveVector.first, allianceFlippedDriveVector.second, angularVelocity, - odometryPose.rotation + odomTRobot.rotation ) } else { desiredChassisSpeeds = @@ -587,24 +543,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 } @@ -637,7 +609,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB } fun addVisionData(visionData: List) { - swerveDrivePoseEstimator.addVisionData(visionData) + fieldFrameEstimator.addVisionData(visionData) } fun lockWheels() { From 78fff09e99e6480e8f6783dca9ae2301933a93d9 Mon Sep 17 00:00:00 2001 From: AlphaPranav9102 <49816864+AlphaPranav9102@users.noreply.github.com> Date: Tue, 9 Jan 2024 00:06:29 -0500 Subject: [PATCH 3/5] refactored to primary vision system for photonvision --- .../subsystems/vision/camera/CameraIOPhotonvision.kt | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt new file mode 100644 index 00000000..15eeeb04 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.vision.camera + +class CameraIOPhotonvision { +} \ No newline at end of file From 61d9fbd19f4e0acf9e521c64fad3c9fdc525b199 Mon Sep 17 00:00:00 2001 From: AlphaPranav9102 <49816864+AlphaPranav9102@users.noreply.github.com> Date: Tue, 9 Jan 2024 00:08:34 -0500 Subject: [PATCH 4/5] removed northstar instances --- .../vision/camera/CameraIONorthstar.kt | 80 ------------------- 1 file changed, 80 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIONorthstar.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIONorthstar.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIONorthstar.kt deleted file mode 100644 index 81c53e67..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIONorthstar.kt +++ /dev/null @@ -1,80 +0,0 @@ -package com.team4099.robot2023.subsystems.vision.camera - -import com.team4099.robot2023.util.Alert -import edu.wpi.first.networktables.DoubleArraySubscriber -import edu.wpi.first.networktables.IntegerSubscriber -import edu.wpi.first.networktables.NetworkTableInstance -import edu.wpi.first.networktables.PubSubOption -import edu.wpi.first.wpilibj.Timer -import org.team4099.lib.units.base.Time -import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.micro - -class CameraIONorthstar(val identifier: String) : CameraIO { - private var cameraId = 0 - private var cameraResolutionWidth = 1600 - private var cameraResolutionHeight = 1200 - private var cameraAutoExposure = 1 - private var cameraExposure = 10 - private var cameraGain = 25 - - private val observationSubscriber: DoubleArraySubscriber - private val fpsSubscriber: IntegerSubscriber - - private val disconnectedTimeout = 0.5 - private val disconnectedAlert: Alert - private val disconnectedTimer = Timer() - - init { - val northStarTable = NetworkTableInstance.getDefault().getTable(identifier) - - val configTable = northStarTable.getSubTable("config") - - configTable.getIntegerTopic("camera_id").publish().set(cameraId.toLong()) - configTable - .getIntegerTopic("camera_resolution_width") - .publish() - .set(cameraResolutionWidth.toLong()) - configTable - .getIntegerTopic("camera_resolution_height") - .publish() - .set(cameraResolutionHeight.toLong()) - configTable.getIntegerTopic("camera_auto_exposure").publish().set(cameraAutoExposure.toLong()) - configTable.getIntegerTopic("camera_exposure").publish().set(cameraExposure.toLong()) - configTable.getIntegerTopic("camera_gain").publish().set(cameraGain.toLong()) - - val outputTable = northStarTable.getSubTable("output") - observationSubscriber = - outputTable - .getDoubleArrayTopic("observations") - .subscribe( - DoubleArray(0), PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true) - ) - fpsSubscriber = outputTable.getIntegerTopic("fps").subscribe(0) - - disconnectedAlert = Alert("No data from $identifier", Alert.AlertType.ERROR) - disconnectedTimer.start() - } - - override fun updateInputs(inputs: CameraIO.CameraInputs) { - val queue = observationSubscriber.readQueue() - val timestamps = mutableListOf