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

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