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

[wpimath] Position Delta Odometry for Mecanum (#4514)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:59 -04:00
committed by GitHub
parent 4170ec6107
commit 901fc555f4
28 changed files with 1222 additions and 235 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -14,9 +14,12 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.numbers.N7;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.function.BiConsumer;
@@ -29,7 +32,7 @@ import java.util.function.BiConsumer;
* <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If your loops are
* faster or slower than the default of 20 ms, then you should change the nominal delta time using
* the secondary constructor: {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d,
* Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
* Pose2d, MecanumDriveWheelPositions, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
*
* <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.
@@ -37,19 +40,21 @@ import java.util.function.BiConsumer;
* <p>The state-space system used internally has the following states (x), inputs (u), and outputs
* (y):
*
* <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
* position, and heading.
* <p><strong> x = [x, y, theta, s_fl, s_fr, s_rl, s_rr]ᵀ </strong> in the field coordinate system
* containing x position, y position, and heading, followed by the distance driven by the front
* left, front right, rear left, and rear right wheels.
*
* <p><strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity, and angular rate
* in the field coordinate system.
* <p><strong> u = [v_x, v_y, omega, v_fl, v_fr, v_rl, v_rr]ᵀ </strong> containing x velocity, y
* velocity, and angular rate in the field coordinate system, followed by the velocity of the front
* left, front right, rear left, and rear right wheels.
*
* <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
* heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
*/
public class MecanumDrivePoseEstimator {
private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
private final UnscentedKalmanFilter<N7, N7, N5> m_observer;
private final MecanumDriveKinematics m_kinematics;
private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
private final BiConsumer<Matrix<N7, N1>, Matrix<N3, N1>> m_visionCorrect;
private final TimeInterpolatableBuffer<Pose2d> m_poseBuffer;
private final double m_nominalDt; // Seconds
@@ -65,13 +70,14 @@ public class MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param wheelPositions The distances driven by each wheel.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
* meters and radians.
* model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
* s_rr]ᵀ, with units in meters and radians, followed by meters.
* @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number
* to trust sensor readings from the gyro less. This matrix is in the form [theta], with units
* in radians.
* to trust sensor readings from the gyro less. This matrix is in the form [theta, s_fl, s_fr,
* s_rl, s_rr], with units in radians, followed by meters.
* @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.
@@ -79,13 +85,15 @@ public class MecanumDrivePoseEstimator {
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
MecanumDriveWheelPositions wheelPositions,
MecanumDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs,
Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N7, N1> stateStdDevs,
Matrix<N5, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
this(
gyroAngle,
initialPoseMeters,
wheelPositions,
kinematics,
stateStdDevs,
localMeasurementStdDevs,
@@ -98,13 +106,14 @@ public class MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param wheelPositions The distances driven by each wheel.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in radians.
* model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
* s_rr]ᵀ, with units in meters and radians, followed by meters.
* @param localMeasurementStdDevs Standard deviation of the gyro measurement. Increase this number
* to trust sensor readings from the gyro less. This matrix is in the form [theta, s_fl, s_fr,
* s_rl, s_rr], with units in radians, followed by meters.
* @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.
@@ -113,19 +122,20 @@ public class MecanumDrivePoseEstimator {
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
MecanumDriveWheelPositions wheelPositions,
MecanumDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs,
Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N7, N1> stateStdDevs,
Matrix<N5, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs,
double nominalDtSeconds) {
m_nominalDt = nominalDtSeconds;
m_observer =
new UnscentedKalmanFilter<>(
Nat.N3(),
Nat.N1(),
Nat.N7(),
Nat.N5(),
(x, u) -> u,
(x, u) -> x.extractRowVector(2),
(x, u) -> x.block(Nat.N5(), Nat.N1(), 2, 0),
stateStdDevs,
localMeasurementStdDevs,
AngleStatistics.angleMean(2),
@@ -146,7 +156,7 @@ public class MecanumDrivePoseEstimator {
Nat.N3(),
u,
y,
(x, u1) -> x,
(x, u1) -> x.block(Nat.N3(), Nat.N1(), 0, 0),
m_visionContR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
@@ -155,7 +165,19 @@ public class MecanumDrivePoseEstimator {
m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
var poseVec = StateSpaceUtil.poseTo3dVector(initialPoseMeters);
var xhat =
VecBuilder.fill(
poseVec.get(0, 0),
poseVec.get(1, 0),
poseVec.get(2, 0),
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
m_observer.setXhat(xhat);
}
/**
@@ -181,13 +203,26 @@ public class MecanumDrivePoseEstimator {
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
public void resetPosition(
Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
// Reset state estimate and error covariance
m_observer.reset();
m_poseBuffer.clear();
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
var poseVec = StateSpaceUtil.poseTo3dVector(poseMeters);
var xhat =
VecBuilder.fill(
poseVec.get(0, 0),
poseVec.get(1, 0),
poseVec.get(2, 0),
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
m_observer.setXhat(xhat);
m_prevTimeSeconds = -1;
@@ -227,7 +262,7 @@ public class MecanumDrivePoseEstimator {
var sample = m_poseBuffer.getSample(timestampSeconds);
if (sample.isPresent()) {
m_visionCorrect.accept(
new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.0, 0.0, 0.0),
new MatBuilder<>(Nat.N7(), Nat.N1()).fill(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
StateSpaceUtil.poseTo3dVector(
getEstimatedPosition().transformBy(visionRobotPoseMeters.minus(sample.get()))));
}
@@ -273,10 +308,14 @@ public class MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
public Pose2d update(
Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds,
MecanumDriveWheelPositions wheelPositions) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds, wheelPositions);
}
/**
@@ -287,10 +326,14 @@ public class MecanumDrivePoseEstimator {
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances driven by each wheel.
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
double currentTimeSeconds,
Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds,
MecanumDriveWheelPositions wheelPositions) {
double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
m_prevTimeSeconds = currentTimeSeconds;
@@ -302,10 +345,24 @@ public class MecanumDrivePoseEstimator {
new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
.rotateBy(angle);
var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
var u =
VecBuilder.fill(
fieldRelativeVelocities.getX(),
fieldRelativeVelocities.getY(),
omega,
wheelSpeeds.frontLeftMetersPerSecond,
wheelSpeeds.frontRightMetersPerSecond,
wheelSpeeds.rearLeftMetersPerSecond,
wheelSpeeds.rearRightMetersPerSecond);
m_previousAngle = angle;
var localY = VecBuilder.fill(angle.getRadians());
var localY =
VecBuilder.fill(
angle.getRadians(),
wheelPositions.frontLeftMeters,
wheelPositions.frontRightMeters,
wheelPositions.rearLeftMeters,
wheelPositions.rearRightMeters);
m_poseBuffer.addSample(currentTimeSeconds, getEstimatedPosition());
m_observer.predict(u, dt);
m_observer.correct(u, localY);

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

@@ -7,6 +7,7 @@ package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import org.ejml.simple.SimpleMatrix;
/**
@@ -153,6 +154,28 @@ 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.
*/
public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) {
var wheelDeltasVector = new SimpleMatrix(4, 1);
wheelDeltasVector.setColumn(
0,
0,
wheelDeltas.frontLeftMeters,
wheelDeltas.frontRightMeters,
wheelDeltas.rearLeftMeters,
wheelDeltas.rearRightMeters);
var twist = m_forwardKinematics.mult(wheelDeltasVector);
return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
}
/**
* Construct inverse kinematics matrix from wheel locations.
*

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

@@ -8,8 +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;
import edu.wpi.first.util.WPIUtilJNI;
/**
* Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
@@ -21,7 +19,7 @@ import edu.wpi.first.util.WPIUtilJNI;
public class MecanumDriveOdometry {
private final MecanumDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private MecanumDriveWheelPositions m_previousWheelPositions;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
@@ -31,14 +29,24 @@ public class MecanumDriveOdometry {
*
* @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 MecanumDriveOdometry(
MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
MecanumDriveKinematics kinematics,
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);
MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
}
@@ -47,9 +55,13 @@ public class MecanumDriveOdometry {
*
* @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 MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
this(kinematics, gyroAngle, new Pose2d());
public MecanumDriveOdometry(
MecanumDriveKinematics kinematics,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions) {
this(kinematics, gyroAngle, wheelPositions, new Pose2d());
}
/**
@@ -60,11 +72,19 @@ public class MecanumDriveOdometry {
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
public void resetPosition(
Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
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);
}
/**
@@ -78,48 +98,37 @@ public class MecanumDriveOdometry {
/**
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method takes in the current time as a parameter to calculate period (difference
* between two timestamps). The period is used to calculate the change in distance from a
* velocity. This also takes in an angle parameter which is used instead of the angular rate that
* is calculated from forward kinematics.
* 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 currentTimeSeconds The current time in seconds.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @param wheelPositions The distances driven by each wheel.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
var angle = gyroAngle.plus(m_gyroOffset);
var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
var newPose =
m_poseMeters.exp(
new Twist2d(
chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
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;
}
/**
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method automatically calculates the current time to calculate period
* (difference between two timestamps). The period is used to calculate the change in distance
* from a velocity. This also takes in an angle parameter which is used instead of the angular
* rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
}
}

49
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java Normal file
View File

@@ -0,0 +1,49 @@
// 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;
public class MecanumDriveWheelPositions {
/** Distance measured by the front left wheel. */
public double frontLeftMeters;
/** Distance measured by the front right wheel. */
public double frontRightMeters;
/** Distance measured by the rear left wheel. */
public double rearLeftMeters;
/** Distance measured by the rear right wheel. */
public double rearRightMeters;
/** Constructs a MecanumDriveWheelPositions with zeros for all member fields. */
public MecanumDriveWheelPositions() {}
/**
* Constructs a MecanumDriveWheelPositions.
*
* @param frontLeftMeters Distance measured by the front left wheel.
* @param frontRightMeters Distance measured by the front right wheel.
* @param rearLeftMeters Distance measured by the rear left wheel.
* @param rearRightMeters Distance measured by the rear right wheel.
*/
public MecanumDriveWheelPositions(
double frontLeftMeters,
double frontRightMeters,
double rearLeftMeters,
double rearRightMeters) {
this.frontLeftMeters = frontLeftMeters;
this.frontRightMeters = frontRightMeters;
this.rearLeftMeters = rearLeftMeters;
this.rearRightMeters = rearRightMeters;
}
@Override
public String toString() {
return String.format(
"MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
+ "Rear Left: %.2f m, Rear Right: %.2f m)",
frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
}
}