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] Add 3D odometry and pose estimation (#7119)

Browse Source
This commit is contained in:
Joseph Eng
2024-11-16 07:56:14 -08:00
committed by GitHub
parent aa7dd258c4
commit 2acf111f56
49 changed files with 6716 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

159
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java Normal file
View File

@@ -0,0 +1,159 @@
// 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.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry3d;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N4;
/**
* This class wraps {@link DifferentialDriveOdometry3d Differential Drive Odometry} to fuse
* latency-compensated vision measurements with differential drive encoder measurements. It is
* intended to be a drop-in replacement for {@link DifferentialDriveOdometry3d}; in fact, if you
* never call {@link DifferentialDrivePoseEstimator3d#addVisionMeasurement} and only call {@link
* DifferentialDrivePoseEstimator3d#update} then this will behave exactly the same as
* DifferentialDriveOdometry3d. It is also intended to be an easy replacement for {@link
* DifferentialDrivePoseEstimator}, only requiring the addition of a standard deviation for Z and
* appropriate conversions between 2D and 3D versions of geometry classes. (See {@link
* Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
*
* <p>{@link DifferentialDrivePoseEstimator3d#update} should be called every robot loop.
*
* <p>{@link DifferentialDrivePoseEstimator3d#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 DifferentialDrivePoseEstimator3d
extends PoseEstimator3d<DifferentialDriveWheelPositions> {
/**
* Constructs a DifferentialDrivePoseEstimator3d with default standard deviations for the model
* and vision measurements.
*
* <p>The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for
* y, 0.02 meters for z, and 0.01 radians for angle. The default standard deviations of the vision
* measurements are 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for
* angle.
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting pose estimate.
*/
public DifferentialDrivePoseEstimator3d(
DifferentialDriveKinematics kinematics,
Rotation3d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose3d initialPoseMeters) {
this(
kinematics,
gyroAngle,
leftDistanceMeters,
rightDistanceMeters,
initialPoseMeters,
VecBuilder.fill(0.02, 0.02, 0.02, 0.01),
VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
}
/**
* Constructs a DifferentialDrivePoseEstimator3d.
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The gyro angle of the robot.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The estimated initial pose.
* @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 DifferentialDrivePoseEstimator3d(
DifferentialDriveKinematics kinematics,
Rotation3d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose3d initialPoseMeters,
Matrix<N4, N1> stateStdDevs,
Matrix<N4, N1> visionMeasurementStdDevs) {
super(
kinematics,
new DifferentialDriveOdometry3d(
gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
}
/**
* 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 leftPositionMeters The distance traveled by the left encoder.
* @param rightPositionMeters The distance traveled by the right encoder.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
Rotation3d gyroAngle,
double leftPositionMeters,
double rightPositionMeters,
Pose3d poseMeters) {
resetPosition(
gyroAngle,
new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters),
poseMeters);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param gyroAngle The current gyro angle.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters.
* @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
public Pose3d update(
Rotation3d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
return update(
gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
}
/**
* 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 distanceLeftMeters The total distance travelled by the left wheel in meters.
* @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
public Pose3d updateWithTime(
double currentTimeSeconds,
Rotation3d gyroAngle,
double distanceLeftMeters,
double distanceRightMeters) {
return updateWithTime(
currentTimeSeconds,
gyroAngle,
new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
}
}

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

@@ -18,7 +18,7 @@ import edu.wpi.first.math.numbers.N3;
* This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated
* vision measurements with mecanum drive encoder distance measurements. It will correct for noisy
* measurements and encoder drift. It is intended to be a drop-in replacement for {@link
* edu.wpi.first.math.kinematics.MecanumDriveOdometry}.
* MecanumDriveOdometry}.
*
* <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop.
*

92
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java Normal file
View File

@@ -0,0 +1,92 @@
// 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.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveOdometry3d;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N4;
/**
* This class wraps {@link MecanumDriveOdometry3d Mecanum Drive Odometry} to fuse
* latency-compensated vision measurements with mecanum drive encoder distance measurements. It will
* correct for noisy measurements and encoder drift. It is intended to be a drop-in replacement for
* {@link MecanumDriveOdometry3d}. It is also intended to be an easy replacement for {@link
* MecanumDrivePoseEstimator}, only requiring the addition of a standard deviation for Z and
* appropriate conversions between 2D and 3D versions of geometry classes. (See {@link
* Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
*
* <p>{@link MecanumDrivePoseEstimator3d#update} should be called every robot loop.
*
* <p>{@link MecanumDrivePoseEstimator3d#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 MecanumDrivePoseEstimator3d extends PoseEstimator3d<MecanumDriveWheelPositions> {
/**
* Constructs a MecanumDrivePoseEstimator3d with default standard deviations for the model and
* vision measurements.
*
* <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
* 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision
* measurements are 0.45 meters for x, 0.45 meters for y, 0.45 meters for z, and 0.45 radians for
* angle.
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting pose estimate.
*/
public MecanumDrivePoseEstimator3d(
MecanumDriveKinematics kinematics,
Rotation3d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose3d initialPoseMeters) {
this(
kinematics,
gyroAngle,
wheelPositions,
initialPoseMeters,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.45, 0.45, 0.45, 0.45));
}
/**
* Constructs a MecanumDrivePoseEstimator3d.
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distance measured by each wheel.
* @param initialPoseMeters The starting pose estimate.
* @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 MecanumDrivePoseEstimator3d(
MecanumDriveKinematics kinematics,
Rotation3d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose3d initialPoseMeters,
Matrix<N4, N1> stateStdDevs,
Matrix<N4, N1> visionMeasurementStdDevs) {
super(
kinematics,
new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
}
}

424
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java Normal file
View File

@@ -0,0 +1,424 @@
// 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.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.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Twist3d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.Kinematics;
import edu.wpi.first.math.kinematics.Odometry3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.math.numbers.N6;
import java.util.NavigableMap;
import java.util.Optional;
import java.util.TreeMap;
/**
* This class wraps {@link Odometry3d} 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 DifferentialDrivePoseEstimator3d}). It is intended to be a drop-in
* replacement for {@link Odometry3d}; in fact, if you never call {@link
* PoseEstimator3d#addVisionMeasurement} and only call {@link PoseEstimator3d#update} then this will
* behave exactly the same as Odometry3d. It is also intended to be an easy replacement for {@link
* PoseEstimator}, only requiring the addition of a standard deviation for Z and appropriate
* conversions between 2D and 3D versions of geometry classes. (See {@link Pose3d#Pose3d(Pose2d)},
* {@link Rotation3d#Rotation3d(Rotation2d)}, {@link Translation3d#Translation3d(Translation2d)},
* and {@link Pose3d#toPose2d()}.)
*
* <p>{@link PoseEstimator3d#update} should be called every robot loop.
*
* <p>{@link PoseEstimator3d#addVisionMeasurement} can be called as infrequently as you want; if you
* never call it then this class will behave exactly like regular encoder odometry.
*
* @param <T> Wheel positions type.
*/
public class PoseEstimator3d<T> {
private final Odometry3d<T> m_odometry;
private final Matrix<N4, N1> m_q = new Matrix<>(Nat.N4(), Nat.N1());
private final Matrix<N6, N6> m_visionK = new Matrix<>(Nat.N6(), Nat.N6());
private static final double kBufferDuration = 1.5;
// Maps timestamps to odometry-only pose estimates
private final TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer =
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
// been no vision measurements after the last reset
private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();
private Pose3d m_poseEstimate;
/**
* Constructs a PoseEstimator3d.
*
* @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, z position in meters, and angle 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, z position in meters, and angle in radians). Increase
* these numbers to trust the vision pose measurement less.
*/
@SuppressWarnings("PMD.UnusedFormalParameter")
public PoseEstimator3d(
Kinematics<?, T> kinematics,
Odometry3d<T> odometry,
Matrix<N4, N1> stateStdDevs,
Matrix<N4, N1> visionMeasurementStdDevs) {
m_odometry = odometry;
m_poseEstimate = m_odometry.getPoseMeters();
for (int i = 0; i < 4; ++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, z,
* theta]ᵀ, with units in meters and radians.
*/
public final void setVisionMeasurementStdDevs(Matrix<N4, N1> visionMeasurementStdDevs) {
var r = new double[4];
for (int i = 0; i < 4; ++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 < 4; ++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])));
}
}
// Fill in the gains for the other components of the rotation vector
double angle_gain = m_visionK.get(3, 3);
m_visionK.set(4, 4, angle_gain);
m_visionK.set(5, 5, angle_gain);
}
/**
* 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(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) {
// Reset state estimate and error covariance
m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
}
/**
* Resets the robot's pose.
*
* @param pose The pose to reset to.
*/
public void resetPose(Pose3d pose) {
m_odometry.resetPose(pose);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
}
/**
* Resets the robot's translation.
*
* @param translation The pose to translation to.
*/
public void resetTranslation(Translation3d translation) {
m_odometry.resetTranslation(translation);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
}
/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
public void resetRotation(Rotation3d rotation) {
m_odometry.resetRotation(rotation);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
}
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
public Pose3d getEstimatedPosition() {
return m_poseEstimate;
}
/**
* Return the pose at a given timestamp, if the buffer is not empty.
*
* @param timestampSeconds The pose's timestamp in seconds.
* @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
*/
public Optional<Pose3d> sampleAt(double timestampSeconds) {
// Step 0: If there are no odometry updates to sample, skip.
if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
return Optional.empty();
}
// Step 1: Make sure timestamp matches the sample from the odometry pose buffer. (When sampling,
// the buffer will always use a timestamp between the first and last timestamps)
double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey();
timestampSeconds =
MathUtil.clamp(timestampSeconds, oldestOdometryTimestamp, newestOdometryTimestamp);
// Step 2: If there are no applicable vision updates, use the odometry-only information.
if (m_visionUpdates.isEmpty() || timestampSeconds < m_visionUpdates.firstKey()) {
return m_odometryPoseBuffer.getSample(timestampSeconds);
}
// Step 3: Get the latest vision update from before or at the timestamp to sample at.
double floorTimestamp = m_visionUpdates.floorKey(timestampSeconds);
var visionUpdate = m_visionUpdates.get(floorTimestamp);
// Step 4: Get the pose measured by odometry at the time of the sample.
var odometryEstimate = m_odometryPoseBuffer.getSample(timestampSeconds);
// Step 5: Apply the vision compensation to the odometry pose.
return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose));
}
/** Removes stale vision updates that won't affect sampling. */
private void cleanUpVisionUpdates() {
// Step 0: If there are no odometry samples, skip.
if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
return;
}
// Step 1: Find the oldest timestamp that needs a vision update.
double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
// Step 2: If there are no vision updates before that timestamp, skip.
if (m_visionUpdates.isEmpty() || oldestOdometryTimestamp < m_visionUpdates.firstKey()) {
return;
}
// Step 3: Find the newest vision update timestamp before or at the oldest timestamp.
double newestNeededVisionUpdateTimestamp = m_visionUpdates.floorKey(oldestOdometryTimestamp);
// Step 4: Remove all entries strictly before the newest timestamp we need.
m_visionUpdates.headMap(newestNeededVisionUpdateTimestamp, false).clear();
}
/**
* 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
* PoseEstimator3d#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
* PoseEstimator3d#updateWithTime(double,Rotation3d,Object)} 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(Pose3d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()
|| m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration
> timestampSeconds) {
return;
}
// Step 1: Clean up any old entries
cleanUpVisionUpdates();
// Step 2: Get the pose measured by odometry at the moment the vision measurement was made.
var odometrySample = m_odometryPoseBuffer.getSample(timestampSeconds);
if (odometrySample.isEmpty()) {
return;
}
// Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was
// made.
var visionSample = sampleAt(timestampSeconds);
if (visionSample.isEmpty()) {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision pose.
var twist = visionSample.get().log(visionRobotPoseMeters);
// Step 5: 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.dz, twist.rx, twist.ry, twist.rz));
// Step 6: Convert back to Twist3d.
var scaledTwist =
new Twist3d(
k_times_twist.get(0, 0),
k_times_twist.get(1, 0),
k_times_twist.get(2, 0),
k_times_twist.get(3, 0),
k_times_twist.get(4, 0),
k_times_twist.get(5, 0));
// Step 7: Calculate and record the vision update.
var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get());
m_visionUpdates.put(timestampSeconds, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)
m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear();
// Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
// it's guaranteed to be the latest vision update.
m_poseEstimate = visionUpdate.compensate(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
* PoseEstimator3d#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
* PoseEstimator3d#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, z position in meters, and angle in radians). Increase
* these numbers to trust the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose3d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N4, 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 Pose3d update(Rotation3d 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 Pose3d updateWithTime(double currentTimeSeconds, Rotation3d gyroAngle, T wheelPositions) {
var odometryEstimate = m_odometry.update(gyroAngle, wheelPositions);
m_odometryPoseBuffer.addSample(currentTimeSeconds, odometryEstimate);
if (m_visionUpdates.isEmpty()) {
m_poseEstimate = odometryEstimate;
} else {
var visionUpdate = m_visionUpdates.get(m_visionUpdates.lastKey());
m_poseEstimate = visionUpdate.compensate(odometryEstimate);
}
return getEstimatedPosition();
}
/**
* Represents a vision update record. The record contains the vision-compensated pose estimate as
* well as the corresponding odometry pose estimate.
*/
private static final class VisionUpdate {
// The vision-compensated pose estimate.
private final Pose3d visionPose;
// The pose estimated based solely on odometry.
private final Pose3d odometryPose;
/**
* Constructs a vision update record with the specified parameters.
*
* @param visionPose The vision-compensated pose estimate.
* @param odometryPose The pose estimate based solely on odometry.
*/
private VisionUpdate(Pose3d visionPose, Pose3d odometryPose) {
this.visionPose = visionPose;
this.odometryPose = odometryPose;
}
/**
* Returns the vision-compensated version of the pose. Specifically, changes the pose from being
* relative to this record's odometry pose to being relative to this record's vision pose.
*
* @param pose The pose to compensate.
* @return The compensated pose.
*/
public Pose3d compensate(Pose3d pose) {
var delta = pose.minus(this.odometryPose);
return this.visionPose.plus(delta);
}
}
}

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

@@ -17,7 +17,7 @@ import edu.wpi.first.math.numbers.N3;
/**
* This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated
* vision measurements with swerve drive encoder distance measurements. It is intended to be a
* drop-in replacement for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
* drop-in replacement for {@link SwerveDriveOdometry}.
*
* <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop.
*

107
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java Normal file
View File

@@ -0,0 +1,107 @@
// 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.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry3d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N4;
/**
* This class wraps {@link SwerveDriveOdometry3d Swerve Drive Odometry} to fuse latency-compensated
* vision measurements with swerve drive encoder distance measurements. It is intended to be a
* drop-in replacement for {@link SwerveDriveOdometry3d}. It is also intended to be an easy
* replacement for {@link SwerveDrivePoseEstimator}, only requiring the addition of a standard
* deviation for Z and appropriate conversions between 2D and 3D versions of geometry classes. (See
* {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
*
* <p>{@link SwerveDrivePoseEstimator3d#update} should be called every robot loop.
*
* <p>{@link SwerveDrivePoseEstimator3d#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 SwerveDrivePoseEstimator3d extends PoseEstimator3d<SwerveModulePosition[]> {
private final int m_numModules;
/**
* Constructs a SwerveDrivePoseEstimator3d with default standard deviations for the model and
* vision measurements.
*
* <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
* 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision
* measurements are 0.9 meters for x, 0.9 meters for y, 0.9 meters for z, and 0.9 radians for
* angle.
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param modulePositions The current distance measurements and rotations of the swerve modules.
* @param initialPoseMeters The starting pose estimate.
*/
public SwerveDrivePoseEstimator3d(
SwerveDriveKinematics kinematics,
Rotation3d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose3d initialPoseMeters) {
this(
kinematics,
gyroAngle,
modulePositions,
initialPoseMeters,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9, 0.9));
}
/**
* Constructs a SwerveDrivePoseEstimator3d.
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param modulePositions The current distance and rotation measurements of the swerve modules.
* @param initialPoseMeters The starting pose estimate.
* @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 SwerveDrivePoseEstimator3d(
SwerveDriveKinematics kinematics,
Rotation3d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose3d initialPoseMeters,
Matrix<N4, N1> stateStdDevs,
Matrix<N4, N1> visionMeasurementStdDevs) {
super(
kinematics,
new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
m_numModules = modulePositions.length;
}
@Override
public Pose3d updateWithTime(
double currentTimeSeconds, Rotation3d gyroAngle, SwerveModulePosition[] wheelPositions) {
if (wheelPositions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
}
}

149
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java Normal file
View File

@@ -0,0 +1,149 @@
// 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 static edu.wpi.first.units.Units.Meters;
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.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.Distance;
/**
* Class for differential drive odometry. Odometry allows you to track the robot's position on the
* field over the course of a match using readings from 2 encoders and a gyroscope.
*
* <p>This class is meant to be an easy replacement for {@link DifferentialDriveOdometry}, only
* requiring the addition of appropriate conversions between 2D and 3D versions of geometry classes.
* (See {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
*
* <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.
*
* <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 DifferentialDriveOdometry3d extends Odometry3d<DifferentialDriveWheelPositions> {
/**
* Constructs a DifferentialDriveOdometry3d object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public DifferentialDriveOdometry3d(
Rotation3d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose3d initialPoseMeters) {
super(
new DifferentialDriveKinematics(1),
gyroAngle,
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
initialPoseMeters);
MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
}
/**
* Constructs a DifferentialDriveOdometry3d object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public DifferentialDriveOdometry3d(
Rotation3d gyroAngle,
Distance leftDistance,
Distance rightDistance,
Pose3d initialPoseMeters) {
this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters);
}
/**
* Constructs a DifferentialDriveOdometry3d object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
*/
public DifferentialDriveOdometry3d(
Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose3d.kZero);
}
/**
* Constructs a DifferentialDriveOdometry3d object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*/
public DifferentialDriveOdometry3d(
Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance) {
this(gyroAngle, leftDistance, rightDistance, Pose3d.kZero);
}
/**
* 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 leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
Rotation3d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose3d poseMeters) {
super.resetPosition(
gyroAngle,
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
poseMeters);
}
/**
* 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 leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance, Pose3d poseMeters) {
resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters);
}
/**
* Updates the robot position on the field using distance measurements from encoders. This method
* is more numerically accurate than using velocities to integrate the pose and is also
* advantageous for teams that are using lower CPR encoders.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
public Pose3d update(
Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
return super.update(
gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
}
}

59
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java Normal file
View File

@@ -0,0 +1,59 @@
// 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.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
/**
* Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
* over a course of a match using readings from your mecanum wheel encoders.
*
* <p>This class is meant to be an easy replacement for {@link MecanumDriveOdometry}, only requiring
* the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See
* {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
*
* <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 MecanumDriveOdometry3d extends Odometry3d<MecanumDriveWheelPositions> {
/**
* Constructs a MecanumDriveOdometry3d object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public MecanumDriveOdometry3d(
MecanumDriveKinematics kinematics,
Rotation3d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose3d initialPoseMeters) {
super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
}
/**
* Constructs a MecanumDriveOdometry3d object with the default pose at the origin.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
*/
public MecanumDriveOdometry3d(
MecanumDriveKinematics kinematics,
Rotation3d gyroAngle,
MecanumDriveWheelPositions wheelPositions) {
this(kinematics, gyroAngle, wheelPositions, Pose3d.kZero);
}
}

148
wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java Normal file
View File

@@ -0,0 +1,148 @@
// 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.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Twist3d;
/**
* Class for odometry. Robot code should not use this directly- Instead, use the particular type for
* your drivetrain (e.g., {@link DifferentialDriveOdometry3d}). 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>This class is meant to be an easy replacement for {@link Odometry}, only requiring the
* addition of appropriate conversions between 2D and 3D versions of geometry classes. (See {@link
* Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
*
* <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.
*
* @param <T> Wheel positions type.
*/
public class Odometry3d<T> {
private final Kinematics<?, T> m_kinematics;
private Pose3d m_poseMeters;
private Rotation3d m_gyroOffset;
private Rotation3d m_previousAngle;
private final T m_previousWheelPositions;
/**
* Constructs an Odometry3d 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 Odometry3d(
Kinematics<?, T> kinematics,
Rotation3d gyroAngle,
T wheelPositions,
Pose3d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = m_poseMeters.getRotation();
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
}
/**
* 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(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = m_poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
}
/**
* Resets the pose.
*
* @param poseMeters The pose to reset to.
*/
public void resetPose(Pose3d poseMeters) {
m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation()));
m_poseMeters = poseMeters;
m_previousAngle = m_poseMeters.getRotation();
}
/**
* Resets the translation of the pose.
*
* @param translation The translation to reset to.
*/
public void resetTranslation(Translation3d translation) {
m_poseMeters = new Pose3d(translation, m_poseMeters.getRotation());
}
/**
* Resets the rotation of the pose.
*
* @param rotation The rotation to reset to.
*/
public void resetRotation(Rotation3d rotation) {
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation);
m_previousAngle = m_poseMeters.getRotation();
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x, y, and z are in meters).
*/
public Pose3d 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 Pose3d update(Rotation3d gyroAngle, T wheelPositions) {
var angle = gyroAngle.plus(m_gyroOffset);
var angle_difference = angle.minus(m_previousAngle).getQuaternion().toRotationVector();
var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
var twist =
new Twist3d(
twist2d.dx,
twist2d.dy,
0,
angle_difference.get(0),
angle_difference.get(1),
angle_difference.get(2));
var newPose = m_poseMeters.exp(twist);
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
m_previousAngle = angle;
m_poseMeters = new Pose3d(newPose.getTranslation(), angle);
return m_poseMeters;
}
}

86
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java Normal file
View File

@@ -0,0 +1,86 @@
// 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.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
/**
* Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
* over a course of a match using readings from your swerve drive encoders and swerve azimuth
* encoders.
*
* <p>This class is meant to be an easy replacement for {@link SwerveDriveOdometry}, only requiring
* the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See
* {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
* Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
*
* <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 SwerveDriveOdometry3d extends Odometry3d<SwerveModulePosition[]> {
private final int m_numModules;
/**
* Constructs a SwerveDriveOdometry3d object.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The wheel positions reported by each module.
* @param initialPose The starting position of the robot on the field.
*/
public SwerveDriveOdometry3d(
SwerveDriveKinematics kinematics,
Rotation3d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose3d initialPose) {
super(kinematics, gyroAngle, modulePositions, initialPose);
m_numModules = modulePositions.length;
MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
}
/**
* Constructs a SwerveDriveOdometry3d object with the default pose at the origin.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The wheel positions reported by each module.
*/
public SwerveDriveOdometry3d(
SwerveDriveKinematics kinematics,
Rotation3d gyroAngle,
SwerveModulePosition[] modulePositions) {
this(kinematics, gyroAngle, modulePositions, Pose3d.kZero);
}
@Override
public void resetPosition(
Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d pose) {
if (modulePositions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
super.resetPosition(gyroAngle, modulePositions, pose);
}
@Override
public Pose3d update(Rotation3d gyroAngle, SwerveModulePosition[] modulePositions) {
if (modulePositions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
return super.update(gyroAngle, modulePositions);
}
}