Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02: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);
}
}

81
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -13,32 +13,47 @@ using namespace frc;
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const MecanumDriveWheelPositions& wheelPositions,
MecanumDriveKinematics kinematics,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 7>& stateStdDevs,
const wpi::array<double, 5>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt)
: m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; },
[](const Vectord<3>& x, const Vectord<3>& u) {
return x.block<1, 1>(2, 0);
: m_observer([](const Vectord<7>& x, const Vectord<7>& u) { return u; },
[](const Vectord<7>& x, const Vectord<7>& u) {
return x.block<5, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
stateStdDevs, localMeasurementStdDevs, frc::AngleMean<7, 7>(2),
frc::AngleMean<5, 7>(0), frc::AngleResidual<7>(2),
frc::AngleResidual<5>(0), frc::AngleAdd<7>(2), nominalDt),
m_kinematics(kinematics),
m_nominalDt(nominalDt) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create vision correction mechanism.
m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
m_visionCorrect = [&](const Vectord<7>& u, const Vectord<3>& y) {
m_observer.Correct<3>(
u, y, [](const Vectord<3>& x, const Vectord<3>&) { return x; },
m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
u, y,
[](const Vectord<7>& x, const Vectord<7>& u) {
return x.template block<3, 1>(0, 0);
},
m_visionContR, frc::AngleMean<3, 7>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<7>(2), frc::AngleAdd<7>(2));
};
// Set initial state.
m_observer.SetXhat(PoseTo3dVector(initialPose));
auto poseVec = PoseTo3dVector(initialPose);
auto xhat = Vectord<7>{
poseVec(0),
poseVec(1),
poseVec(2),
wheelPositions.frontLeft.value(),
wheelPositions.frontRight.value(),
wheelPositions.rearLeft.value(),
wheelPositions.rearRight.value(),
};
m_observer.SetXhat(xhat);
// Calculate offsets.
m_gyroOffset = initialPose.Rotation() - gyroAngle;
@@ -52,12 +67,24 @@ void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
}
void frc::MecanumDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();
m_observer.SetXhat(PoseTo3dVector(pose));
auto poseVec = PoseTo3dVector(pose);
auto xhat = Vectord<7>{
poseVec(0),
poseVec(1),
poseVec(2),
wheelPositions.frontLeft.value(),
wheelPositions.frontRight.value(),
wheelPositions.rearLeft.value(),
wheelPositions.rearRight.value(),
};
m_observer.SetXhat(xhat);
m_prevTime = -1_s;
@@ -73,21 +100,23 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Vectord<3>::Zero(),
m_visionCorrect(Vectord<7>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
}
Pose2d frc::MecanumDrivePoseEstimator::Update(
const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds) {
const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
wheelSpeeds);
wheelSpeeds, wheelPositions);
}
Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds) {
const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions) {
auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
m_prevTime = currentTime;
@@ -99,10 +128,18 @@ Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
Translation2d{chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s}.RotateBy(
angle);
Vectord<3> u{fieldRelativeVelocities.X().value(),
fieldRelativeVelocities.Y().value(), omega.value()};
Vectord<7> u{fieldRelativeVelocities.X().value(),
fieldRelativeVelocities.Y().value(),
omega.value(),
wheelSpeeds.frontLeft.value(),
wheelSpeeds.frontRight.value(),
wheelSpeeds.rearLeft.value(),
wheelSpeeds.rearRight.value()};
Vectord<1> localY{angle.Radians().value()};
Vectord<5> localY{angle.Radians().value(), wheelPositions.frontLeft.value(),
wheelPositions.frontRight.value(),
wheelPositions.rearLeft.value(),
wheelPositions.rearRight.value()};
m_previousAngle = angle;
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());

12
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -49,6 +49,18 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
Twist2d MecanumDriveKinematics::ToTwist2d(
const MecanumDriveWheelPositions& wheelDeltas) const {
Vectord<4> wheelDeltasVector{
wheelDeltas.frontLeft.value(), wheelDeltas.frontRight.value(),
wheelDeltas.rearLeft.value(), wheelDeltas.rearRight.value()};
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
return {units::meter_t{twistVector(0)}, // NOLINT
units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}};
}
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
Translation2d fr,
Translation2d rl,

35
wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
View File

@@ -8,32 +8,37 @@
using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
MecanumDriveOdometry::MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions, const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
}
const Pose2d& MecanumDriveOdometry::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
const Pose2d& MecanumDriveOdometry::Update(
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
static_cast<void>(dtheta);
MecanumDriveWheelPositions wheelDeltas{
wheelPositions.frontLeft - m_previousWheelPositions.frontLeft,
wheelPositions.frontRight - m_previousWheelPositions.frontRight,
wheelPositions.rearLeft - m_previousWheelPositions.rearLeft,
wheelPositions.rearRight - m_previousWheelPositions.rearRight,
};
auto newPose = m_pose.Exp(
{dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
auto twist = m_kinematics.ToTwist2d(wheelDeltas);
twist.dtheta = (angle - m_previousAngle).Radians();
auto newPose = m_pose.Exp(twist);
m_previousAngle = angle;
m_previousWheelPositions = wheelPositions;
m_pose = {newPose.Translation(), angle};
return m_pose;

49
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -35,11 +35,15 @@ namespace frc {
* The state-space system used internally has the following states (x), inputs
* (u), and outputs (y):
*
* <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
* containing x position, y position, and heading.
* <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.
*
* <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
* and angular velocity in the field coordinate system.
* <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.
*
* <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
* position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
@@ -52,17 +56,21 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param initialPose The starting pose estimate.
* @param wheelPositions The distance measured 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 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.
* 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
@@ -74,9 +82,10 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
*/
MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const MecanumDriveWheelPositions& wheelPositions,
MecanumDriveKinematics kinematics,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, 7>& stateStdDevs,
const wpi::array<double, 5>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 20_ms);
@@ -105,8 +114,10 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances measured at each wheel.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions);
/**
* Gets the pose of the robot at the current time as estimated by the Extended
@@ -190,10 +201,12 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
*
* @param gyroAngle The current gyro angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param wheelPositions The distances measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds);
const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions);
/**
* Updates the the Unscented Kalman Filter using only wheel encoder
@@ -203,17 +216,19 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
* @param currentTime 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 measured at each wheel.
* @return The estimated pose of the robot in meters.
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
const MecanumDriveWheelSpeeds& wheelSpeeds);
const MecanumDriveWheelSpeeds& wheelSpeeds,
const MecanumDriveWheelPositions& wheelPositions);
private:
UnscentedKalmanFilter<3, 3, 1> m_observer;
UnscentedKalmanFilter<7, 7, 5> m_observer;
MecanumDriveKinematics m_kinematics;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
std::function<void(const Vectord<7>& u, const Vectord<3>& y)> m_visionCorrect;
Eigen::Matrix3d m_visionContR;

14
wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
View File

@@ -9,7 +9,9 @@
#include "Eigen/QR"
#include "frc/EigenCore.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/MecanumDriveWheelPositions.h"
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include "wpimath/MathShared.h"
@@ -114,6 +116,18 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
ChassisSpeeds ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const;
/**
* Performs forward kinematics to return the resulting Twist2d from the
* given wheel position deltas. This method is often used for odometry --
* determining the robot's position on the field using data from the
* distance driven by each wheel on the robot.
*
* @param wheelDeltas The change in distance driven by each wheel.
*
* @return The resulting chassis speed.
*/
Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
private:
mutable Matrixd<4, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;

41
wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
View File

@@ -30,10 +30,12 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions,
const Pose2d& initialPose = Pose2d{});
/**
@@ -44,11 +46,14 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current distances measured by each wheel.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_previousWheelPositions = wheelPositions;
}
/**
@@ -59,45 +64,23 @@ class WPILIB_DLLEXPORT 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.
*
* @param currentTime The current time.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds);
/**
* 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.
* 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 wheelSpeeds The current wheel speeds.
* @param wheelPositions The current distances measured by each wheel.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds);
}
const MecanumDriveWheelPositions wheelPositions);
private:
MecanumDriveKinematics m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
MecanumDriveWheelPositions m_previousWheelPositions;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
};

36
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h Normal file
View File

@@ -0,0 +1,36 @@
// 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.
#pragma once
#include <wpi/SymbolExports.h>
#include "units/length.h"
namespace frc {
/**
* Represents the wheel speeds for a mecanum drive drivetrain.
*/
struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
/**
* Distance driven by the front-left wheel.
*/
units::meter_t frontLeft = 0_m;
/**
* Distance driven by the front-right wheel.
*/
units::meter_t frontRight = 0_m;
/**
* Distance driven by the rear-left wheel.
*/
units::meter_t rearLeft = 0_m;
/**
* Distance driven by the rear-right wheel.
*/
units::meter_t rearRight = 0_m;
};
} // namespace frc