Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Refactor kinematics, odometry, and pose estimator (#5355)

Browse Source
This commit is contained in:
Joseph Eng
2023-06-19 17:10:39 -07:00
committed by GitHub
parent 5c2addda0f
commit 25ad5017a9
41 changed files with 1742 additions and 2177 deletions
Download Patch File Download Diff File Expand all files Collapse all files

290
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -4,23 +4,15 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
/**
* This class wraps {@link DifferentialDriveOdometry Differential Drive Odometry} to fuse
@@ -35,17 +27,7 @@ import java.util.Objects;
* <p>{@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as
* you want; if you never call it then this class will behave exactly like regular encoder odometry.
*/
public class DifferentialDrivePoseEstimator {
private final DifferentialDriveKinematics m_kinematics;
private final DifferentialDriveOdometry m_odometry;
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private static final double kBufferDuration = 1.5;
private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDriveWheelPositions> {
/**
* Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
* vision measurements.
@@ -99,44 +81,12 @@ public class DifferentialDrivePoseEstimator {
Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
m_kinematics = kinematics;
m_odometry =
super(
kinematics,
new DifferentialDriveOdometry(
gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters);
for (int i = 0; i < 3; ++i) {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
}
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
/**
* Sets the pose estimator's trust of global measurements. This might be used to change trust in
* vision measurements after the autonomous period, or to change trust as distance to a vision
* target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var r = new double[3];
for (int i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (int row = 0; row < 3; ++row) {
if (m_q.get(row, 0) == 0.0) {
m_visionK.set(row, row, 0.0);
} else {
m_visionK.set(
row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
}
}
gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
}
/**
@@ -155,129 +105,10 @@ public class DifferentialDrivePoseEstimator {
double leftPositionMeters,
double rightPositionMeters,
Pose2d poseMeters) {
// Reset state estimate and error covariance
m_odometry.resetPosition(gyroAngle, leftPositionMeters, rightPositionMeters, poseMeters);
m_poseBuffer.clear();
}
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
public Pose2d getEstimatedPosition() {
return m_odometry.getPoseMeters();
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* DifferentialDrivePoseEstimator#update} every loop.
*
* <p>To promote stability of the pose estimate and make it robust to bad vision data, we
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)} then you
* must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
* the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
* you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
* or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
try {
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
} catch (NoSuchElementException ex) {
return;
}
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
var sample = m_poseBuffer.getSample(timestampSeconds);
if (sample.isEmpty()) {
return;
}
// Step 2: Measure the twist between the odometry pose and the vision pose.
var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
// Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
// Step 4: Convert back to Twist2d.
var scaledTwist =
new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
// Step 5: Reset Odometry to state at sample with vision adjustment.
m_odometry.resetPosition(
sample.get().gyroAngle,
sample.get().leftMeters,
sample.get().rightMeters,
sample.get().poseMeters.exp(scaledTwist));
// Step 6: Record the current pose to allow multiple measurements from the same timestamp
m_poseBuffer.addSample(
timestampSeconds,
new InterpolationRecord(
getEstimatedPosition(),
sample.get().gyroAngle,
sample.get().leftMeters,
sample.get().rightMeters));
// Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
// pose buffer and correct odometry.
for (Map.Entry<Double, InterpolationRecord> entry :
m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
updateWithTime(
entry.getKey(),
entry.getValue().gyroAngle,
entry.getValue().leftMeters,
entry.getValue().rightMeters);
}
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* DifferentialDrivePoseEstimator#update} every loop.
*
* <p>To promote stability of the pose estimate and make it robust to bad vision data, we
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)}, then you
* must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
* the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that
* you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
* in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
* in meters, y position in meters, and heading in radians). Increase these numbers to trust
* the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
resetPosition(
gyroAngle,
new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters),
poseMeters);
}
/**
@@ -291,8 +122,8 @@ public class DifferentialDrivePoseEstimator {
*/
public Pose2d update(
Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
return updateWithTime(
MathSharedStore.getTimestamp(), gyroAngle, distanceLeftMeters, distanceRightMeters);
return update(
gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
}
/**
@@ -310,98 +141,9 @@ public class DifferentialDrivePoseEstimator {
Rotation2d gyroAngle,
double distanceLeftMeters,
double distanceRightMeters) {
m_odometry.update(gyroAngle, distanceLeftMeters, distanceRightMeters);
m_poseBuffer.addSample(
return updateWithTime(
currentTimeSeconds,
new InterpolationRecord(
getEstimatedPosition(), gyroAngle, distanceLeftMeters, distanceRightMeters));
return getEstimatedPosition();
}
/**
* Represents an odometry record. The record contains the inputs provided as well as the pose that
* was observed based on these inputs, as well as the previous record and its inputs.
*/
private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
// The pose observed given the current sensor inputs and the previous pose.
private final Pose2d poseMeters;
// The current gyro angle.
private final Rotation2d gyroAngle;
// The distance traveled by the left encoder.
private final double leftMeters;
// The distance traveled by the right encoder.
private final double rightMeters;
/**
* Constructs an Interpolation Record with the specified parameters.
*
* @param poseMeters The pose observed given the current sensor inputs and the previous pose.
* @param gyro The current gyro angle.
* @param leftMeters The distance traveled by the left encoder.
* @param rightMeters The distanced traveled by the right encoder.
*/
private InterpolationRecord(
Pose2d poseMeters, Rotation2d gyro, double leftMeters, double rightMeters) {
this.poseMeters = poseMeters;
this.gyroAngle = gyro;
this.leftMeters = leftMeters;
this.rightMeters = rightMeters;
}
/**
* Return the interpolated record. This object is assumed to be the starting position, or lower
* bound.
*
* @param endValue The upper bound, or end.
* @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
* @return The interpolated value.
*/
@Override
public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
if (t < 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
// Find the new left distance.
var left_lerp = MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t);
// Find the new right distance.
var right_lerp = MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t);
// Find the new gyro angle.
var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
// Create a twist to represent this change based on the interpolated sensor inputs.
Twist2d twist = m_kinematics.toTwist2d(left_lerp - leftMeters, right_lerp - rightMeters);
twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, left_lerp, right_lerp);
}
}
@Override
public boolean equals(Object obj) {
if (this == obj) {
return true;
}
if (!(obj instanceof InterpolationRecord)) {
return false;
}
InterpolationRecord record = (InterpolationRecord) obj;
return Objects.equals(gyroAngle, record.gyroAngle)
&& Double.compare(leftMeters, record.leftMeters) == 0
&& Double.compare(rightMeters, record.rightMeters) == 0
&& Objects.equals(poseMeters, record.poseMeters);
}
@Override
public int hashCode() {
return Objects.hash(gyroAngle, leftMeters, rightMeters, poseMeters);
}
gyroAngle,
new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
}
}

330
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
View File

@@ -4,24 +4,15 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
/**
* This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated
@@ -34,17 +25,7 @@ import java.util.Objects;
* <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
*/
public class MecanumDrivePoseEstimator {
private final MecanumDriveKinematics m_kinematics;
private final MecanumDriveOdometry m_odometry;
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private static final double kBufferDuration = 1.5;
private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
public class MecanumDrivePoseEstimator extends PoseEstimator<MecanumDriveWheelPositions> {
/**
* Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and
* vision measurements.
@@ -93,309 +74,10 @@ public class MecanumDrivePoseEstimator {
Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
m_kinematics = kinematics;
m_odometry = new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
for (int i = 0; i < 3; ++i) {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
}
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
/**
* Sets the pose estimator's trust of global measurements. This might be used to change trust in
* vision measurements after the autonomous period, or to change trust as distance to a vision
* target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var r = new double[3];
for (int i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (int row = 0; row < 3; ++row) {
if (m_q.get(row, 0) == 0.0) {
m_visionK.set(row, row, 0.0);
} else {
m_visionK.set(
row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
}
}
}
/**
* Resets the robot's position on the field.
*
* <p>The gyroscope angle does not need to be reset in the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
m_poseBuffer.clear();
}
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
public Pose2d getEstimatedPosition() {
return m_odometry.getPoseMeters();
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* MecanumDrivePoseEstimator#update} every loop.
*
* <p>To promote stability of the pose estimate and make it robust to bad vision data, we
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)}
* then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
* timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.)
* This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
* your time source or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
try {
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
} catch (NoSuchElementException ex) {
return;
}
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
var sample = m_poseBuffer.getSample(timestampSeconds);
if (sample.isEmpty()) {
return;
}
// Step 2: Measure the twist between the odometry pose and the vision pose.
var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
// Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
// Step 4: Convert back to Twist2d.
var scaledTwist =
new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
// Step 5: Reset Odometry to state at sample with vision adjustment.
m_odometry.resetPosition(
sample.get().gyroAngle,
sample.get().wheelPositions,
sample.get().poseMeters.exp(scaledTwist));
// Step 6: Record the current pose to allow multiple measurements from the same timestamp
m_poseBuffer.addSample(
timestampSeconds,
new InterpolationRecord(
getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions));
// Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
// pose buffer and correct odometry.
for (Map.Entry<Double, InterpolationRecord> entry :
m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
}
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* MecanumDrivePoseEstimator#update} every loop.
*
* <p>To promote stability of the pose estimate and make it robust to bad vision data, we
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* MecanumDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)},
* then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
* timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
* This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
* your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
* in meters, y position in meters, and heading in radians). Increase these numbers to trust
* the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param wheelPositions The distances driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
m_odometry.update(gyroAngle, wheelPositions);
m_poseBuffer.addSample(
currentTimeSeconds,
new InterpolationRecord(
getEstimatedPosition(),
gyroAngle,
new MecanumDriveWheelPositions(
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters)));
return getEstimatedPosition();
}
/**
* Represents an odometry record. The record contains the inputs provided as well as the pose that
* was observed based on these inputs, as well as the previous record and its inputs.
*/
private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
// The pose observed given the current sensor inputs and the previous pose.
private final Pose2d poseMeters;
// The current gyro angle.
private final Rotation2d gyroAngle;
// The distances traveled by each wheel encoder.
private final MecanumDriveWheelPositions wheelPositions;
/**
* Constructs an Interpolation Record with the specified parameters.
*
* @param poseMeters The pose observed given the current sensor inputs and the previous pose.
* @param gyro The current gyro angle.
* @param wheelPositions The distances traveled by each wheel encoder.
*/
private InterpolationRecord(
Pose2d poseMeters, Rotation2d gyro, MecanumDriveWheelPositions wheelPositions) {
this.poseMeters = poseMeters;
this.gyroAngle = gyro;
this.wheelPositions = wheelPositions;
}
/**
* Return the interpolated record. This object is assumed to be the starting position, or lower
* bound.
*
* @param endValue The upper bound, or end.
* @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
* @return The interpolated value.
*/
@Override
public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
if (t < 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
// Find the new wheel distances.
var wheels_lerp =
new MecanumDriveWheelPositions(
MathUtil.interpolate(
this.wheelPositions.frontLeftMeters,
endValue.wheelPositions.frontLeftMeters,
t),
MathUtil.interpolate(
this.wheelPositions.frontRightMeters,
endValue.wheelPositions.frontRightMeters,
t),
MathUtil.interpolate(
this.wheelPositions.rearLeftMeters, endValue.wheelPositions.rearLeftMeters, t),
MathUtil.interpolate(
this.wheelPositions.rearRightMeters,
endValue.wheelPositions.rearRightMeters,
t));
// Find the distance travelled between this measurement and the interpolated measurement.
var wheels_delta =
new MecanumDriveWheelPositions(
wheels_lerp.frontLeftMeters - this.wheelPositions.frontLeftMeters,
wheels_lerp.frontRightMeters - this.wheelPositions.frontRightMeters,
wheels_lerp.rearLeftMeters - this.wheelPositions.rearLeftMeters,
wheels_lerp.rearRightMeters - this.wheelPositions.rearRightMeters);
// Find the new gyro angle.
var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
// Create a twist to represent this change based on the interpolated sensor inputs.
Twist2d twist = m_kinematics.toTwist2d(wheels_delta);
twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, wheels_lerp);
}
}
@Override
public boolean equals(Object obj) {
if (this == obj) {
return true;
}
if (!(obj instanceof InterpolationRecord)) {
return false;
}
InterpolationRecord record = (InterpolationRecord) obj;
return Objects.equals(gyroAngle, record.gyroAngle)
&& Objects.equals(wheelPositions, record.wheelPositions)
&& Objects.equals(poseMeters, record.poseMeters);
}
@Override
public int hashCode() {
return Objects.hash(gyroAngle, wheelPositions, poseMeters);
}
super(
kinematics,
new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
}
}

336
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java Normal file
View File

@@ -0,0 +1,336 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.Kinematics;
import edu.wpi.first.math.kinematics.Odometry;
import edu.wpi.first.math.kinematics.WheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
/**
* This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder
* measurements. Robot code should not use this directly- Instead, use the particular type for your
* drivetrain (e.g., {@link DifferentialDrivePoseEstimator}). It is intended to be a drop-in
* replacement for {@link Odometry}; in fact, if you never call {@link
* PoseEstimator#addVisionMeasurement} and only call {@link PoseEstimator#update} then this will
* behave exactly the same as Odometry.
*
* <p>{@link PoseEstimator#update} should be called every robot loop.
*
* <p>{@link PoseEstimator#addVisionMeasurement} can be called as infrequently as you want; if you
* never call it then this class will behave exactly like regular encoder odometry.
*/
public class PoseEstimator<T extends WheelPositions<T>> {
private final Kinematics<?, T> m_kinematics;
private final Odometry<T> m_odometry;
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
private final Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private static final double kBufferDuration = 1.5;
private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
/**
* Constructs a PoseEstimator.
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param odometry A correctly-configured odometry object for your drivetrain.
* @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
* in meters, and heading in radians). Increase these numbers to trust your state estimate
* less.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
* in meters, y position in meters, and heading in radians). Increase these numbers to trust
* the vision pose measurement less.
*/
public PoseEstimator(
Kinematics<?, T> kinematics,
Odometry<T> odometry,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
m_kinematics = kinematics;
m_odometry = odometry;
for (int i = 0; i < 3; ++i) {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
}
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
/**
* Sets the pose estimator's trust of global measurements. This might be used to change trust in
* vision measurements after the autonomous period, or to change trust as distance to a vision
* target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var r = new double[3];
for (int i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (int row = 0; row < 3; ++row) {
if (m_q.get(row, 0) == 0.0) {
m_visionK.set(row, row, 0.0);
} else {
m_visionK.set(
row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
}
}
}
/**
* Resets the robot's position on the field.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
m_poseBuffer.clear();
}
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
public Pose2d getEstimatedPosition() {
return m_odometry.getPoseMeters();
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* PoseEstimator#update} every loop.
*
* <p>To promote stability of the pose estimate and make it robust to bad vision data, we
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* PoseEstimator#updateWithTime(double,Rotation2d,WheelPositions)} then you must use a
* timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
* epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you
* should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or
* sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
try {
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
} catch (NoSuchElementException ex) {
return;
}
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
var sample = m_poseBuffer.getSample(timestampSeconds);
if (sample.isEmpty()) {
return;
}
// Step 2: Measure the twist between the odometry pose and the vision pose.
var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
// Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
// Step 4: Convert back to Twist2d.
var scaledTwist =
new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
// Step 5: Reset Odometry to state at sample with vision adjustment.
m_odometry.resetPosition(
sample.get().gyroAngle,
sample.get().wheelPositions,
sample.get().poseMeters.exp(scaledTwist));
// Step 6: Record the current pose to allow multiple measurements from the same timestamp
m_poseBuffer.addSample(
timestampSeconds,
new InterpolationRecord(
getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions));
// Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
// pose buffer and correct odometry.
for (Map.Entry<Double, InterpolationRecord> entry :
m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions);
}
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* PoseEstimator#update} every loop.
*
* <p>To promote stability of the pose estimate and make it robust to bad vision data, we
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* PoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link #updateWithTime}, then you must use a
* timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
* epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you
* should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in
* this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
* in meters, y position in meters, and heading in radians). Increase these numbers to trust
* the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param gyroAngle The current gyro angle.
* @param wheelPositions The current encoder readings.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The current encoder readings.
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) {
m_odometry.update(gyroAngle, wheelPositions);
m_poseBuffer.addSample(
currentTimeSeconds,
new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy()));
return getEstimatedPosition();
}
/**
* Represents an odometry record. The record contains the inputs provided as well as the pose that
* was observed based on these inputs, as well as the previous record and its inputs.
*/
private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
// The pose observed given the current sensor inputs and the previous pose.
private final Pose2d poseMeters;
// The current gyro angle.
private final Rotation2d gyroAngle;
// The current encoder readings.
private final T wheelPositions;
/**
* Constructs an Interpolation Record with the specified parameters.
*
* @param poseMeters The pose observed given the current sensor inputs and the previous pose.
* @param gyro The current gyro angle.
* @param wheelPositions The current encoder readings.
*/
private InterpolationRecord(Pose2d poseMeters, Rotation2d gyro, T wheelPositions) {
this.poseMeters = poseMeters;
this.gyroAngle = gyro;
this.wheelPositions = wheelPositions;
}
/**
* Return the interpolated record. This object is assumed to be the starting position, or lower
* bound.
*
* @param endValue The upper bound, or end.
* @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
* @return The interpolated value.
*/
@Override
public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
if (t < 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
// Find the new wheel distances.
var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t);
// Find the distance travelled between this measurement and the interpolated measurement.
var wheelDelta = wheelLerp.minus(wheelPositions);
// Find the new gyro angle.
var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t);
// Create a twist to represent this change based on the interpolated sensor inputs.
Twist2d twist = m_kinematics.toTwist2d(wheelDelta);
twist.dtheta = gyroLerp.minus(gyroAngle).getRadians();
return new InterpolationRecord(poseMeters.exp(twist), gyroLerp, wheelLerp);
}
}
@Override
public boolean equals(Object obj) {
if (this == obj) {
return true;
}
if (!(obj instanceof PoseEstimator.InterpolationRecord)) {
return false;
}
var record = (PoseEstimator<?>.InterpolationRecord) obj;
return Objects.equals(gyroAngle, record.gyroAngle)
&& Objects.equals(wheelPositions, record.wheelPositions)
&& Objects.equals(poseMeters, record.poseMeters);
}
@Override
public int hashCode() {
return Objects.hash(gyroAngle, wheelPositions, poseMeters);
}
}
}

297
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
View File

@@ -4,25 +4,16 @@
package edu.wpi.first.math.estimator;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import java.util.Arrays;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
/**
* This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
@@ -34,17 +25,8 @@ import java.util.Objects;
* <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave as regular encoder odometry.
*/
public class SwerveDrivePoseEstimator {
private final SwerveDriveKinematics m_kinematics;
private final SwerveDriveOdometry m_odometry;
private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveDriveWheelPositions> {
private final int m_numModules;
private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
private static final double kBufferDuration = 1.5;
private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
/**
* Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
@@ -94,43 +76,13 @@ public class SwerveDrivePoseEstimator {
Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
m_kinematics = kinematics;
m_odometry = new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters);
for (int i = 0; i < 3; ++i) {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
}
super(
kinematics,
new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
m_numModules = modulePositions.length;
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
/**
* Sets the pose estimator's trust of global measurements. This might be used to change trust in
* vision measurements after the autonomous period, or to change trust as distance to a vision
* target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]ᵀ, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var r = new double[3];
for (int i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0);
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (int row = 0; row < 3; ++row) {
if (m_q.get(row, 0) == 0.0) {
m_visionK.set(row, row, 0.0);
} else {
m_visionK.set(
row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row])));
}
}
}
/**
@@ -145,121 +97,7 @@ public class SwerveDrivePoseEstimator {
*/
public void resetPosition(
Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters);
m_poseBuffer.clear();
}
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
public Pose2d getEstimatedPosition() {
return m_odometry.getPoseMeters();
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* SwerveDrivePoseEstimator#update} every loop.
*
* <p>To promote stability of the pose estimate and make it robust to bad vision data, we
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])} then you
* must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is
* the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that
* you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source
* or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
try {
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
} catch (NoSuchElementException ex) {
return;
}
// Step 1: Get the pose odometry measured at the moment the vision measurement was made.
var sample = m_poseBuffer.getSample(timestampSeconds);
if (sample.isEmpty()) {
return;
}
// Step 2: Measure the twist between the odometry pose and the vision pose.
var twist = sample.get().poseMeters.log(visionRobotPoseMeters);
// Step 3: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
var k_times_twist = m_visionK.times(VecBuilder.fill(twist.dx, twist.dy, twist.dtheta));
// Step 4: Convert back to Twist2d.
var scaledTwist =
new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
// Step 5: Reset Odometry to state at sample with vision adjustment.
m_odometry.resetPosition(
sample.get().gyroAngle,
sample.get().modulePositions,
sample.get().poseMeters.exp(scaledTwist));
// Step 6: Record the current pose to allow multiple measurements from the same timestamp
m_poseBuffer.addSample(
timestampSeconds,
new InterpolationRecord(
getEstimatedPosition(), sample.get().gyroAngle, sample.get().modulePositions));
// Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
// pose buffer and correct odometry.
for (Map.Entry<Double, InterpolationRecord> entry :
m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {
updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().modulePositions);
}
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* SwerveDrivePoseEstimator#update} every loop.
*
* <p>To promote stability of the pose estimate and make it robust to bad vision data, we
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])}, then
* you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this
* timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}).
* This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as
* your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
* in meters, y position in meters, and heading in radians). Increase these numbers to trust
* the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters);
}
/**
@@ -271,7 +109,7 @@ public class SwerveDrivePoseEstimator {
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, modulePositions);
return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
}
/**
@@ -285,118 +123,19 @@ public class SwerveDrivePoseEstimator {
*/
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
if (modulePositions.length != m_numModules) {
return updateWithTime(
currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions));
}
@Override
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) {
if (wheelPositions.positions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
var internalModulePositions = new SwerveModulePosition[m_numModules];
for (int i = 0; i < m_numModules; i++) {
internalModulePositions[i] =
new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle);
}
m_odometry.update(gyroAngle, internalModulePositions);
m_poseBuffer.addSample(
currentTimeSeconds,
new InterpolationRecord(getEstimatedPosition(), gyroAngle, internalModulePositions));
return getEstimatedPosition();
}
/**
* Represents an odometry record. The record contains the inputs provided as well as the pose that
* was observed based on these inputs, as well as the previous record and its inputs.
*/
private class InterpolationRecord implements Interpolatable<InterpolationRecord> {
// The pose observed given the current sensor inputs and the previous pose.
private final Pose2d poseMeters;
// The current gyro angle.
private final Rotation2d gyroAngle;
// The distances and rotations measured at each module.
private final SwerveModulePosition[] modulePositions;
/**
* Constructs an Interpolation Record with the specified parameters.
*
* @param poseMeters The pose observed given the current sensor inputs and the previous pose.
* @param gyro The current gyro angle.
* @param modulePositions The distances and rotations measured at each wheel.
*/
private InterpolationRecord(
Pose2d poseMeters, Rotation2d gyro, SwerveModulePosition[] modulePositions) {
this.poseMeters = poseMeters;
this.gyroAngle = gyro;
this.modulePositions = modulePositions;
}
/**
* Return the interpolated record. This object is assumed to be the starting position, or lower
* bound.
*
* @param endValue The upper bound, or end.
* @param t How far between the lower and upper bound we are. This should be bounded in [0, 1].
* @return The interpolated value.
*/
@Override
public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
if (t < 0) {
return this;
} else if (t >= 1) {
return endValue;
} else {
// Find the new wheel distances.
var modulePositions = new SwerveModulePosition[m_numModules];
// Find the distance travelled between this measurement and the interpolated measurement.
var moduleDeltas = new SwerveModulePosition[m_numModules];
for (int i = 0; i < m_numModules; i++) {
double ds =
MathUtil.interpolate(
this.modulePositions[i].distanceMeters,
endValue.modulePositions[i].distanceMeters,
t);
Rotation2d theta =
this.modulePositions[i].angle.interpolate(endValue.modulePositions[i].angle, t);
modulePositions[i] = new SwerveModulePosition(ds, theta);
moduleDeltas[i] =
new SwerveModulePosition(ds - this.modulePositions[i].distanceMeters, theta);
}
// Find the new gyro angle.
var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t);
// Create a twist to represent this change based on the interpolated sensor inputs.
Twist2d twist = m_kinematics.toTwist2d(moduleDeltas);
twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians();
return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, modulePositions);
}
}
@Override
public boolean equals(Object obj) {
if (this == obj) {
return true;
}
if (!(obj instanceof InterpolationRecord)) {
return false;
}
InterpolationRecord record = (InterpolationRecord) obj;
return Objects.equals(gyroAngle, record.gyroAngle)
&& Arrays.equals(modulePositions, record.modulePositions)
&& Objects.equals(poseMeters, record.poseMeters);
}
@Override
public int hashCode() {
return Objects.hash(gyroAngle, Arrays.hashCode(modulePositions), poseMeters);
}
return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
}
}

10
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
View File

@@ -16,7 +16,8 @@ import edu.wpi.first.math.geometry.Twist2d;
* whereas forward kinematics converts left and right component velocities into a linear and angular
* chassis speed.
*/
public class DifferentialDriveKinematics {
public class DifferentialDriveKinematics
implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions> {
public final double trackWidthMeters;
/**
@@ -37,6 +38,7 @@ public class DifferentialDriveKinematics {
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
@Override
public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
return new ChassisSpeeds(
(wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
@@ -51,6 +53,7 @@ public class DifferentialDriveKinematics {
* chassis' speed.
* @return The left and right velocities.
*/
@Override
public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return new DifferentialDriveWheelSpeeds(
chassisSpeeds.vxMetersPerSecond
@@ -59,6 +62,11 @@ public class DifferentialDriveKinematics {
+ trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
}
@Override
public Twist2d toTwist2d(DifferentialDriveWheelPositions wheelDeltas) {
return toTwist2d(wheelDeltas.leftMeters, wheelDeltas.rightMeters);
}
/**
* Performs forward kinematics to return the resulting Twist2d from the given left and right side
* distance deltas. This method is often used for odometry -- determining the robot's position on

61
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
View File

@@ -8,7 +8,6 @@ import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
/**
* Class for differential drive odometry. Odometry allows you to track the robot's position on the
@@ -20,15 +19,7 @@ import edu.wpi.first.math.geometry.Twist2d;
* <p>It is important that you reset your encoders to zero before using this class. Any subsequent
* pose resets also require the encoders to be reset to zero.
*/
public class DifferentialDriveOdometry {
private Pose2d m_poseMeters;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private double m_prevLeftDistance;
private double m_prevRightDistance;
public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPositions> {
/**
* Constructs a DifferentialDriveOdometry object.
*
@@ -42,13 +33,11 @@ public class DifferentialDriveOdometry {
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters) {
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
m_prevLeftDistance = leftDistanceMeters;
m_prevRightDistance = rightDistanceMeters;
super(
new DifferentialDriveKinematics(1),
gyroAngle,
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
initialPoseMeters);
MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
}
@@ -80,21 +69,10 @@ public class DifferentialDriveOdometry {
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_prevLeftDistance = leftDistanceMeters;
m_prevRightDistance = rightDistanceMeters;
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
return m_poseMeters;
super.resetPosition(
gyroAngle,
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
poseMeters);
}
/**
@@ -109,22 +87,7 @@ public class DifferentialDriveOdometry {
*/
public Pose2d update(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
m_prevLeftDistance = leftDistanceMeters;
m_prevRightDistance = rightDistanceMeters;
double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
var angle = gyroAngle.plus(m_gyroOffset);
var newPose =
m_poseMeters.exp(
new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
return super.update(
gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
}
}

68
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java Normal file
View File

@@ -0,0 +1,68 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathUtil;
import java.util.Objects;
public class DifferentialDriveWheelPositions
implements WheelPositions<DifferentialDriveWheelPositions> {
/** Distance measured by the left side. */
public double leftMeters;
/** Distance measured by the right side. */
public double rightMeters;
/**
* Constructs a DifferentialDriveWheelPositions.
*
* @param leftMeters Distance measured by the left side.
* @param rightMeters Distance measured by the right side.
*/
public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) {
this.leftMeters = leftMeters;
this.rightMeters = rightMeters;
}
@Override
public boolean equals(Object obj) {
if (obj instanceof DifferentialDriveWheelPositions) {
DifferentialDriveWheelPositions other = (DifferentialDriveWheelPositions) obj;
return Math.abs(other.leftMeters - leftMeters) < 1E-9
&& Math.abs(other.rightMeters - rightMeters) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(leftMeters, rightMeters);
}
@Override
public String toString() {
return String.format(
"DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters);
}
@Override
public DifferentialDriveWheelPositions copy() {
return new DifferentialDriveWheelPositions(leftMeters, rightMeters);
}
@Override
public DifferentialDriveWheelPositions minus(DifferentialDriveWheelPositions other) {
return new DifferentialDriveWheelPositions(
this.leftMeters - other.leftMeters, this.rightMeters - other.rightMeters);
}
@Override
public DifferentialDriveWheelPositions interpolate(
DifferentialDriveWheelPositions endValue, double t) {
return new DifferentialDriveWheelPositions(
MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t),
MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t));
}
}

46
wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java Normal file
View File

@@ -0,0 +1,46 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.geometry.Twist2d;
/**
* Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot
* code should not use this directly- Instead, use the particular type for your drivetrain (e.g.,
* {@link DifferentialDriveKinematics}).
*
* @param <S> The type of the wheel speeds.
* @param <P> The type of the wheel positions.
*/
public interface Kinematics<S, P> {
/**
* Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This
* method is often used for odometry -- determining the robot's position on the field using data
* from the real-world speed of each wheel on the robot.
*
* @param wheelSpeeds The speeds of the wheels.
* @return The chassis speed.
*/
ChassisSpeeds toChassisSpeeds(S wheelSpeeds);
/**
* Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
* method is often used to convert joystick values into wheel speeds.
*
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
*/
S toWheelSpeeds(ChassisSpeeds chassisSpeeds);
/**
* Performs forward kinematics to return the resulting from the given wheel deltas. This method is
* often used for odometry -- determining the robot's position on the field using changes in the
* distance driven by each wheel on the robot.
*
* @param wheelDeltas The distances driven by each wheel.
* @return The resulting Twist2d in the robot's movement.
*/
Twist2d toTwist2d(P wheelDeltas);
}

14
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
View File

@@ -30,7 +30,8 @@ import org.ejml.simple.SimpleMatrix;
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
public class MecanumDriveKinematics {
public class MecanumDriveKinematics
implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
private final SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_forwardKinematics;
@@ -125,6 +126,7 @@ public class MecanumDriveKinematics {
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
*/
@Override
public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return toWheelSpeeds(chassisSpeeds, new Translation2d());
}
@@ -137,6 +139,7 @@ public class MecanumDriveKinematics {
* @param wheelSpeeds The current mecanum drive wheel speeds.
* @return The resulting chassis speed.
*/
@Override
public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
var wheelSpeedsVector = new SimpleMatrix(4, 1);
wheelSpeedsVector.setColumn(
@@ -154,14 +157,7 @@ public class MecanumDriveKinematics {
chassisSpeedsVector.get(2, 0));
}
/**
* Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This
* method is often used for odometry -- determining the robot's position on the field using
* changes in the distance driven by each wheel on the robot.
*
* @param wheelDeltas The distances driven by each wheel.
* @return The resulting Twist2d.
*/
@Override
public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) {
var wheelDeltasVector = new SimpleMatrix(4, 1);
wheelDeltasVector.setColumn(

88
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
View File

@@ -16,14 +16,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
*/
public class MecanumDriveOdometry {
private final MecanumDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
private MecanumDriveWheelPositions m_previousWheelPositions;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
public class MecanumDriveOdometry extends Odometry<MecanumDriveWheelPositions> {
/**
* Constructs a MecanumDriveOdometry object.
*
@@ -37,16 +30,7 @@ public class MecanumDriveOdometry {
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
m_previousWheelPositions =
new MecanumDriveWheelPositions(
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
}
@@ -63,72 +47,4 @@ public class MecanumDriveOdometry {
MecanumDriveWheelPositions wheelPositions) {
this(kinematics, gyroAngle, wheelPositions, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousWheelPositions =
new MecanumDriveWheelPositions(
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method takes in an angle parameter which is used instead of the angular rate
* that is calculated from forward kinematics, in addition to the current distance measurement at
* each wheel.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
var angle = gyroAngle.plus(m_gyroOffset);
var wheelDeltas =
new MecanumDriveWheelPositions(
wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters);
var twist = m_kinematics.toTwist2d(wheelDeltas);
twist.dtheta = angle.minus(m_previousAngle).getRadians();
var newPose = m_poseMeters.exp(twist);
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
m_previousWheelPositions =
new MecanumDriveWheelPositions(
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
return m_poseMeters;
}
}

46
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
View File

@@ -4,7 +4,10 @@
package edu.wpi.first.math.kinematics;
public class MecanumDriveWheelPositions {
import edu.wpi.first.math.MathUtil;
import java.util.Objects;
public class MecanumDriveWheelPositions implements WheelPositions<MecanumDriveWheelPositions> {
/** Distance measured by the front left wheel. */
public double frontLeftMeters;
@@ -39,6 +42,23 @@ public class MecanumDriveWheelPositions {
this.rearRightMeters = rearRightMeters;
}
@Override
public boolean equals(Object obj) {
if (obj instanceof MecanumDriveWheelPositions) {
MecanumDriveWheelPositions other = (MecanumDriveWheelPositions) obj;
return Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9
&& Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9
&& Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9
&& Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
}
@Override
public String toString() {
return String.format(
@@ -46,4 +66,28 @@ public class MecanumDriveWheelPositions {
+ "Rear Left: %.2f m, Rear Right: %.2f m)",
frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
}
@Override
public MecanumDriveWheelPositions copy() {
return new MecanumDriveWheelPositions(
frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
}
@Override
public MecanumDriveWheelPositions minus(MecanumDriveWheelPositions other) {
return new MecanumDriveWheelPositions(
this.frontLeftMeters - other.frontLeftMeters,
this.frontRightMeters - other.frontRightMeters,
this.rearLeftMeters - other.rearLeftMeters,
this.rearRightMeters - other.rearRightMeters);
}
@Override
public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
return new MecanumDriveWheelPositions(
MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t),
MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t),
MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t),
MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t));
}
}

99
wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java Normal file
View File

@@ -0,0 +1,99 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
/**
* Class for odometry. Robot code should not use this directly- Instead, use the particular type for
* your drivetrain (e.g., {@link DifferentialDriveOdometry}). Odometry allows you to track the
* robot's position on the field over the course of a match using readings from encoders and a
* gyroscope.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
*/
public class Odometry<T extends WheelPositions<T>> {
private final Kinematics<?, T> m_kinematics;
private Pose2d m_poseMeters;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private T m_previousWheelPositions;
/**
* Constructs an Odometry object.
*
* @param kinematics The kinematics of the drivebase.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public Odometry(
Kinematics<?, T> kinematics,
Rotation2d gyroAngle,
T wheelPositions,
Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = m_poseMeters.getRotation();
m_previousWheelPositions = wheelPositions.copy();
}
/**
* Resets the robot's position on the field.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = m_poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousWheelPositions = wheelPositions.copy();
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method takes in an angle parameter which is used instead of the angular rate
* that is calculated from forward kinematics, in addition to the current distance measurement at
* each wheel.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
T wheelDeltas = wheelPositions.minus(m_previousWheelPositions);
var angle = gyroAngle.plus(m_gyroOffset);
var twist = m_kinematics.toTwist2d(wheelDeltas);
twist.dtheta = angle.minus(m_previousAngle).getRadians();
var newPose = m_poseMeters.exp(twist);
m_previousWheelPositions = wheelPositions.copy();
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
}
}

34
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -32,7 +32,24 @@ import org.ejml.simple.SimpleMatrix;
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
public class SwerveDriveKinematics {
public class SwerveDriveKinematics
implements Kinematics<SwerveDriveKinematics.SwerveDriveWheelStates, SwerveDriveWheelPositions> {
public static class SwerveDriveWheelStates {
public SwerveModuleState[] states;
/**
* Creates a new SwerveDriveWheelStates instance.
*
* @param states The swerve module states. This will be deeply copied.
*/
public SwerveDriveWheelStates(SwerveModuleState[] states) {
this.states = new SwerveModuleState[states.length];
for (int i = 0; i < states.length; i++) {
this.states[i] = new SwerveModuleState(states[i].speedMetersPerSecond, states[i].angle);
}
}
}
private final SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_forwardKinematics;
@@ -159,6 +176,11 @@ public class SwerveDriveKinematics {
return toSwerveModuleStates(chassisSpeeds, new Translation2d());
}
@Override
public SwerveDriveWheelStates toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return new SwerveDriveWheelStates(toSwerveModuleStates(chassisSpeeds));
}
/**
* Performs forward kinematics to return the resulting chassis state from the given module states.
* This method is often used for odometry -- determining the robot's position on the field using
@@ -190,6 +212,11 @@ public class SwerveDriveKinematics {
chassisSpeedsVector.get(2, 0));
}
@Override
public ChassisSpeeds toChassisSpeeds(SwerveDriveWheelStates wheelStates) {
return toChassisSpeeds(wheelStates.states);
}
/**
* Performs forward kinematics to return the resulting chassis state from the given module states.
* This method is often used for odometry -- determining the robot's position on the field using
@@ -219,6 +246,11 @@ public class SwerveDriveKinematics {
chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
}
@Override
public Twist2d toTwist2d(SwerveDriveWheelPositions wheelDeltas) {
return toTwist2d(wheelDeltas.positions);
}
/**
* Renormalizes the wheel speeds if any individual speed is above the specified maximum.
*

78
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
View File

@@ -17,14 +17,8 @@ import edu.wpi.first.math.geometry.Rotation2d;
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
*/
public class SwerveDriveOdometry {
private final SwerveDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
private final int m_numModules;
private SwerveModulePosition[] m_previousModulePositions;
/**
* Constructs a SwerveDriveOdometry object.
@@ -39,18 +33,9 @@ public class SwerveDriveOdometry {
Rotation2d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose2d initialPose) {
m_kinematics = kinematics;
m_poseMeters = initialPose;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPose.getRotation();
m_numModules = modulePositions.length;
super(kinematics, gyroAngle, new SwerveDriveWheelPositions(modulePositions), initialPose);
m_previousModulePositions = new SwerveModulePosition[m_numModules];
for (int index = 0; index < m_numModules; index++) {
m_previousModulePositions[index] =
new SwerveModulePosition(
modulePositions[index].distanceMeters, modulePositions[index].angle);
}
m_numModules = modulePositions.length;
MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
}
@@ -83,29 +68,18 @@ public class SwerveDriveOdometry {
*/
public void resetPosition(
Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
if (modulePositions.length != m_numModules) {
resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), pose);
}
@Override
public void resetPosition(
Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions, Pose2d pose) {
if (modulePositions.positions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
m_poseMeters = pose;
m_previousAngle = pose.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
for (int index = 0; index < m_numModules; index++) {
m_previousModulePositions[index] =
new SwerveModulePosition(
modulePositions[index].distanceMeters, modulePositions[index].angle);
}
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
return m_poseMeters;
super.resetPosition(gyroAngle, modulePositions, pose);
}
/**
@@ -121,32 +95,16 @@ public class SwerveDriveOdometry {
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
if (modulePositions.length != m_numModules) {
return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
}
@Override
public Pose2d update(Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions) {
if (modulePositions.positions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
var moduleDeltas = new SwerveModulePosition[m_numModules];
for (int index = 0; index < m_numModules; index++) {
var current = modulePositions[index];
var previous = m_previousModulePositions[index];
moduleDeltas[index] =
new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle);
previous.distanceMeters = current.distanceMeters;
}
var angle = gyroAngle.plus(m_gyroOffset);
var twist = m_kinematics.toTwist2d(moduleDeltas);
twist.dtheta = angle.minus(m_previousAngle).getRadians();
var newPose = m_poseMeters.exp(twist);
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
return super.update(gyroAngle, modulePositions);
}
}

72
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java Normal file
View File

@@ -0,0 +1,72 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import java.util.Arrays;
import java.util.Objects;
import java.util.function.BinaryOperator;
public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
public SwerveModulePosition[] positions;
/**
* Creates a new SwerveDriveWheelPositions instance.
*
* @param positions The swerve module positions. This will be deeply copied.
*/
public SwerveDriveWheelPositions(SwerveModulePosition[] positions) {
this.positions = new SwerveModulePosition[positions.length];
for (int i = 0; i < positions.length; i++) {
this.positions[i] = positions[i].copy();
}
}
@Override
public boolean equals(Object obj) {
if (obj instanceof SwerveDriveWheelPositions) {
SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj;
return Arrays.equals(this.positions, other.positions);
}
return false;
}
@Override
public int hashCode() {
// Cast to interpret positions as single argument, not array of the arguments
return Objects.hash((Object) positions);
}
@Override
public String toString() {
return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions));
}
private SwerveDriveWheelPositions generate(
SwerveDriveWheelPositions other, BinaryOperator<SwerveModulePosition> generator) {
if (other.positions.length != positions.length) {
throw new IllegalArgumentException("Inconsistent number of modules!");
}
var newPositions = new SwerveModulePosition[positions.length];
for (int i = 0; i < positions.length; i++) {
newPositions[i] = generator.apply(positions[i], other.positions[i]);
}
return new SwerveDriveWheelPositions(newPositions);
}
@Override
public SwerveDriveWheelPositions copy() {
return new SwerveDriveWheelPositions(positions);
}
@Override
public SwerveDriveWheelPositions minus(SwerveDriveWheelPositions other) {
return generate(other, (a, b) -> a.minus(b));
}
@Override
public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) {
return generate(endValue, (a, b) -> a.interpolate(b, t));
}
}

33
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
View File

@@ -4,11 +4,14 @@
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
/** Represents the state of one swerve module. */
public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
public class SwerveModulePosition
implements Comparable<SwerveModulePosition>, Interpolatable<SwerveModulePosition> {
/** Distance measured by the wheel of the module. */
public double distanceMeters;
@@ -60,4 +63,32 @@ public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
return String.format(
"SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
}
/**
* Returns a copy of this swerve module position.
*
* @return A copy.
*/
public SwerveModulePosition copy() {
return new SwerveModulePosition(distanceMeters, angle);
}
/**
* Calculates the difference between two swerve module positions. The difference has a length
* equal to the difference in lengths and an angle equal to the ending angle (this module
* position's angle).
*
* @param other The swerve module position to subtract.
* @return The difference.
*/
public SwerveModulePosition minus(SwerveModulePosition other) {
return new SwerveModulePosition(this.distanceMeters - other.distanceMeters, this.angle);
}
@Override
public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
return new SwerveModulePosition(
MathUtil.interpolate(this.distanceMeters, endValue.distanceMeters, t),
this.angle.interpolate(endValue.angle, t));
}
}

24
wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java Normal file
View File

@@ -0,0 +1,24 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.interpolation.Interpolatable;
public interface WheelPositions<T extends WheelPositions<T>> extends Interpolatable<T> {
/**
* Returns a copy of this instance.
*
* @return A copy.
*/
T copy();
/**
* Returns the difference with another set of wheel positions.
*
* @param other The other instance to compare to.
* @return The representation of how the wheels moved from other to this.
*/
T minus(T other);
}