Skip to content

Commit

Permalink
Remove 2d overloads, remove 3d suffixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@KangarooKoala
KangarooKoala committed Nov 13, 2024
1 parent 11a836c commit 5a44f3c
Show file tree
Hide file tree
Showing 34 changed files with 989 additions and 2,023 deletions.
131 changes: 8 additions & 123 deletions wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,11 +10,12 @@
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.N3;
import edu.wpi.first.math.numbers.N4;

/**
Expand All @@ -23,7 +24,11 @@
* 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.
* 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.
*
Expand All @@ -32,67 +37,6 @@
*/
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, and 0.01 radians for heading. The default standard deviations of the vision measurements are
* 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
*
* @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,
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters) {
this(
kinematics,
gyroAngle,
leftDistanceMeters,
rightDistanceMeters,
initialPoseMeters,
VecBuilder.fill(0.02, 0.02, 0.01),
VecBuilder.fill(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,
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
super(
kinematics,
new DifferentialDriveOdometry3d(
gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
}

/**
* Constructs a DifferentialDrivePoseEstimator3d with default standard deviations for the model
* and vision measurements.
Expand Down Expand Up @@ -152,30 +96,7 @@ public DifferentialDrivePoseEstimator3d(
new DifferentialDriveOdometry3d(
gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs,
N3.instance);
}

/**
* 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(
Rotation2d gyroAngle,
double leftPositionMeters,
double rightPositionMeters,
Pose2d poseMeters) {
resetPosition(
gyroAngle,
new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters),
poseMeters);
visionMeasurementStdDevs);
}

/**
Expand All @@ -200,21 +121,6 @@ public void resetPosition(
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 Pose2d update(
Rotation2d 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.
Expand All @@ -230,27 +136,6 @@ public Pose3d 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 Pose2d updateWithTime(
double currentTimeSeconds,
Rotation2d gyroAngle,
double distanceLeftMeters,
double distanceRightMeters) {
return updateWithTime(
currentTimeSeconds,
gyroAngle,
new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
}

/**
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
Expand Down
67 changes: 8 additions & 59 deletions wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,80 +10,30 @@
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.N3;
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}.
* {@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,
* and 0.1 radians for heading. The default standard deviations of the vision measurements are
* 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading.
*
* @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,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters) {
this(
kinematics,
gyroAngle,
wheelPositions,
initialPoseMeters,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(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,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
super(
kinematics,
new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs);
}

/**
* Constructs a MecanumDrivePoseEstimator3d with default standard deviations for the model and
* vision measurements.
Expand Down Expand Up @@ -137,7 +87,6 @@ public MecanumDrivePoseEstimator3d(
kinematics,
new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
stateStdDevs,
visionMeasurementStdDevs,
N3.instance);
visionMeasurementStdDevs);
}
}
Loading

0 comments on commit 5a44f3c

Please sign in to comment.