Skip to content

Commit

Permalink
Cache latest aiming parameters if nothing has changed in RobotState
Browse files Browse the repository at this point in the history
  • Loading branch information
@camearle20
camearle20 committed Feb 8, 2024
1 parent 0c6ffb9 commit d14475f
Showing 1 changed file with 183 additions and 165 deletions.
348 changes: 183 additions & 165 deletions src/main/java/frc/robot/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,178 +12,196 @@
import edu.wpi.first.math.numbers.N3;
import frc.robot.subsystems.drive.DriveConstants;
import frc.robot.util.AllianceFlipUtil;

import java.util.NoSuchElementException;

import org.littletonrobotics.junction.AutoLogOutput;

public class RobotState {
// Pose Estimation
public record OdometryObservation(
SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {}

public record VisionObservation(Pose2d visionPose, double timestamp, Matrix<N3, N1> stdDevs) {}

public record AimingParameters(
Rotation2d driveHeading, double effectiveDistance, double radialFF) {}

private static final double poseBufferSizeSeconds = 2.0;

private static RobotState instance;

public static RobotState getInstance() {
if (instance == null) instance = new RobotState();
return instance;
}

// Pose Estimation Members
private Pose2d odometryPose = new Pose2d();
private Pose2d estimatedPose = new Pose2d();
private final TimeInterpolatableBuffer<Pose2d> poseBuffer =
TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds);
private final Matrix<N3, N1> qStdDevs = new Matrix<>(Nat.N3(), Nat.N1());
// odometry
private final SwerveDriveKinematics kinematics;
private SwerveDriveWheelPositions lastWheelPositions =
new SwerveDriveWheelPositions(
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
});
private Rotation2d lastGyroAngle = new Rotation2d();
private Twist2d robotVelocity = new Twist2d();

private RobotState() {
for (int i = 0; i < 3; ++i) {
qStdDevs.set(i, 0, Math.pow(qStdDevs.get(i, 0), 2));
// Pose Estimation
public record OdometryObservation(
SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {
}

public record VisionObservation(Pose2d visionPose, double timestamp, Matrix<N3, N1> stdDevs) {
}

public record AimingParameters(
Rotation2d driveHeading, double effectiveDistance, double radialFF) {
}

private static final double poseBufferSizeSeconds = 2.0;

private static RobotState instance;

public static RobotState getInstance() {
if (instance == null) instance = new RobotState();
return instance;
}

// Pose Estimation Members
private Pose2d odometryPose = new Pose2d();
private Pose2d estimatedPose = new Pose2d();
private final TimeInterpolatableBuffer<Pose2d> poseBuffer =
TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds);
private final Matrix<N3, N1> qStdDevs = new Matrix<>(Nat.N3(), Nat.N1());
// odometry
private final SwerveDriveKinematics kinematics;
private SwerveDriveWheelPositions lastWheelPositions =
new SwerveDriveWheelPositions(
new SwerveModulePosition[]{
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
});
private Rotation2d lastGyroAngle = new Rotation2d();
private Twist2d robotVelocity = new Twist2d();

private AimingParameters latestParameters = null;

private RobotState() {
for (int i = 0; i < 3; ++i) {
qStdDevs.set(i, 0, Math.pow(qStdDevs.get(i, 0), 2));
}
kinematics = DriveConstants.kinematics;
}
kinematics = DriveConstants.kinematics;
}

/** Add odometry observation */
public void addOdometryObservation(OdometryObservation observation) {
Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions());
lastWheelPositions = observation.wheelPositions();
// Check gyro connected
if (observation.gyroAngle != null) {
// Update dtheta for twist if gyro connected
twist =
new Twist2d(
twist.dx, twist.dy, observation.gyroAngle().minus(lastGyroAngle).getRadians());
lastGyroAngle = observation.gyroAngle();

/**
* Add odometry observation
*/
public void addOdometryObservation(OdometryObservation observation) {
latestParameters = null;
Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions());
lastWheelPositions = observation.wheelPositions();
// Check gyro connected
if (observation.gyroAngle != null) {
// Update dtheta for twist if gyro connected
twist =
new Twist2d(
twist.dx, twist.dy, observation.gyroAngle().minus(lastGyroAngle).getRadians());
lastGyroAngle = observation.gyroAngle();
}
// Add twist to odometry pose
odometryPose = odometryPose.exp(twist);
// Add pose to buffer at timestamp
poseBuffer.addSample(observation.timestamp(), odometryPose);
// Calculate diff from last odometry pose and add onto pose estimate
estimatedPose = estimatedPose.exp(twist);
}

public void addVisionObservation(VisionObservation observation) {
latestParameters = null;
// If measurement is old enough to be outside the pose buffer's timespan, skip.
try {
if (poseBuffer.getInternalBuffer().lastKey() - poseBufferSizeSeconds
> observation.timestamp()) {
return;
}
} catch (NoSuchElementException ex) {
return;
}
// Get odometry based pose at timestamp
var sample = poseBuffer.getSample(observation.timestamp());
if (sample.isEmpty())
// exit if not there
return;

// sample --> odometryPose transform and backwards of that
var sampleToOdometryTransform = new Transform2d(sample.get(), odometryPose);
var odometryToSampleTransform = new Transform2d(odometryPose, sample.get());
// get old estimate by applying odometryToSample Transform
Pose2d estimateAtTime = estimatedPose.plus(odometryToSampleTransform);

// Calculate 3 x 3 vision matrix
var r = new double[3];
for (int i = 0; i < 3; ++i) {
r[i] = observation.stdDevs().get(i, 0) * observation.stdDevs().get(i, 0);
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
Matrix<N3, N3> visionK = new Matrix<>(Nat.N3(), Nat.N3());
for (int row = 0; row < 3; ++row) {
double stdDev = qStdDevs.get(row, 0);
if (stdDev == 0.0) {
visionK.set(row, row, 0.0);
} else {
visionK.set(row, row, stdDev / (stdDev + Math.sqrt(stdDev * r[row])));
}
}
// difference between estimate and vision pose
Twist2d twist = estimateAtTime.log(observation.visionPose());
// scale twist by visionK
var kTimesTwist = visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
Twist2d scaledTwist =
new Twist2d(kTimesTwist.get(0, 0), kTimesTwist.get(1, 0), kTimesTwist.get(2, 0));

// Recalculate current estimate by applying scaled twist to old estimate
// then replaying odometry data
estimatedPose = estimateAtTime.exp(scaledTwist).plus(sampleToOdometryTransform);
}

public void addVelocityData(Twist2d robotVelocity) {
this.latestParameters = null;
this.robotVelocity = robotVelocity;
}

public AimingParameters getAimingParameters() {
if (latestParameters != null) {
// Cache previously calculated aiming parameters. Cache is invalidated whenever new observations are added.
return latestParameters;
}

Pose2d robot = getEstimatedPose();
Twist2d fieldVelocity = fieldVelocity();

Translation2d originToGoal =
AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening.getTranslation());
Translation2d originToRobot = robot.getTranslation();

// Get robot to goal angle but limit to reasonable range
Rotation2d robotToGoalAngle = originToGoal.minus(originToGoal).getAngle();
// Subtract goal to robot angle from field velocity
Translation2d tangentialVelocity =
new Translation2d(fieldVelocity.dx, fieldVelocity.dy)
.rotateBy(robotToGoalAngle.unaryMinus());
// Subtract tangential velocity from goal to get virtual goal
Translation2d originToVirtualGoal = originToGoal.plus(tangentialVelocity.unaryMinus());

// Angle to virtual goal
Rotation2d driveHeading = originToVirtualGoal.minus(originToRobot).getAngle();
// Distance to virtual goal
double effectiveDistance = originToRobot.getDistance(originToVirtualGoal);
double radialFF = -tangentialVelocity.getX();
latestParameters = new AimingParameters(driveHeading, effectiveDistance, radialFF);
return latestParameters;
}

/**
* Reset estimated pose and odometry pose to pose <br>
* Clear pose buffer
*/
public void resetPose(Pose2d initialPose) {
estimatedPose = initialPose;
odometryPose = initialPose;
poseBuffer.clear();
}
// Add twist to odometry pose
odometryPose = odometryPose.exp(twist);
// Add pose to buffer at timestamp
poseBuffer.addSample(observation.timestamp(), odometryPose);
// Calculate diff from last odometry pose and add onto pose estimate
estimatedPose = estimatedPose.exp(twist);
}

public void addVisionObservation(VisionObservation observation) {
// If measurement is old enough to be outside the pose buffer's timespan, skip.
try {
if (poseBuffer.getInternalBuffer().lastKey() - poseBufferSizeSeconds
> observation.timestamp()) {
return;
}
} catch (NoSuchElementException ex) {
return;

@AutoLogOutput(key = "Odometry/FieldVelocity")
public Twist2d fieldVelocity() {
Translation2d linearFieldVelocity =
new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation());
return new Twist2d(
linearFieldVelocity.getX(), linearFieldVelocity.getY(), robotVelocity.dtheta);
}
// Get odometry based pose at timestamp
var sample = poseBuffer.getSample(observation.timestamp());
if (sample.isEmpty())
// exit if not there
return;

// sample --> odometryPose transform and backwards of that
var sampleToOdometryTransform = new Transform2d(sample.get(), odometryPose);
var odometryToSampleTransform = new Transform2d(odometryPose, sample.get());
// get old estimate by applying odometryToSample Transform
Pose2d estimateAtTime = estimatedPose.plus(odometryToSampleTransform);

// Calculate 3 x 3 vision matrix
var r = new double[3];
for (int i = 0; i < 3; ++i) {
r[i] = observation.stdDevs().get(i, 0) * observation.stdDevs().get(i, 0);

@AutoLogOutput(key = "Odometry/Robot")
public Pose2d getEstimatedPose() {
return estimatedPose;
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
Matrix<N3, N3> visionK = new Matrix<>(Nat.N3(), Nat.N3());
for (int row = 0; row < 3; ++row) {
double stdDev = qStdDevs.get(row, 0);
if (stdDev == 0.0) {
visionK.set(row, row, 0.0);
} else {
visionK.set(row, row, stdDev / (stdDev + Math.sqrt(stdDev * r[row])));
}

@AutoLogOutput(key = "Odometry/OdometryPose")
public Pose2d getOdometryPose() {
return odometryPose;
}
// difference between estimate and vision pose
Twist2d twist = estimateAtTime.log(observation.visionPose());
// scale twist by visionK
var kTimesTwist = visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
Twist2d scaledTwist =
new Twist2d(kTimesTwist.get(0, 0), kTimesTwist.get(1, 0), kTimesTwist.get(2, 0));

// Recalculate current estimate by applying scaled twist to old estimate
// then replaying odometry data
estimatedPose = estimateAtTime.exp(scaledTwist).plus(sampleToOdometryTransform);
}

public void addVelocityData(Twist2d robotVelocity) {
this.robotVelocity = robotVelocity;
}

public AimingParameters getAimingParameters() {
Pose2d robot = getEstimatedPose();
Twist2d fieldVelocity = fieldVelocity();

Translation2d originToGoal =
AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening.getTranslation());
Translation2d originToRobot = robot.getTranslation();

// Get robot to goal angle but limit to reasonable range
Rotation2d robotToGoalAngle = originToGoal.minus(originToGoal).getAngle();
// Subtract goal to robot angle from field velocity
Translation2d tangentialVelocity =
new Translation2d(fieldVelocity.dx, fieldVelocity.dy)
.rotateBy(robotToGoalAngle.unaryMinus());
// Subtract tangential velocity from goal to get virtual goal
Translation2d originToVirtualGoal = originToGoal.plus(tangentialVelocity.unaryMinus());

// Angle to virtual goal
Rotation2d driveHeading = originToVirtualGoal.minus(originToRobot).getAngle();
// Distance to virtual goal
double effectiveDistance = originToRobot.getDistance(originToVirtualGoal);
double radialFF = -tangentialVelocity.getX();
return new AimingParameters(driveHeading, effectiveDistance, radialFF);
}

/**
* Reset estimated pose and odometry pose to pose <br>
* Clear pose buffer
*/
public void resetPose(Pose2d initialPose) {
estimatedPose = initialPose;
odometryPose = initialPose;
poseBuffer.clear();
}

@AutoLogOutput(key = "Odometry/FieldVelocity")
public Twist2d fieldVelocity() {
Translation2d linearFieldVelocity =
new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation());
return new Twist2d(
linearFieldVelocity.getX(), linearFieldVelocity.getY(), robotVelocity.dtheta);
}

@AutoLogOutput(key = "Odometry/Robot")
public Pose2d getEstimatedPose() {
return estimatedPose;
}

@AutoLogOutput(key = "Odometry/OdometryPose")
public Pose2d getOdometryPose() {
return odometryPose;
}
}

0 comments on commit d14475f

Please sign in to comment.