Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01: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

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