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

[wpimath] Add 3D odometry and pose estimation (#7119)

Browse Source
This commit is contained in:
Joseph Eng
2024-11-16 07:56:14 -08:00
committed by GitHub
parent aa7dd258c4
commit 2acf111f56
49 changed files with 6716 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -21,9 +21,8 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
units::meter_t leftDistance, units::meter_t rightDistance,
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: PoseEstimator<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
: PoseEstimator(kinematics, m_odometryImpl, stateStdDevs,
visionMeasurementStdDevs),
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {
ResetPose(m_odometryImpl.GetPose());
ResetPose(initialPose);
}

27
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp Normal file
View File

@@ -0,0 +1,27 @@
// 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.
#include "frc/estimator/DifferentialDrivePoseEstimator3d.h"
using namespace frc;
DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d(
DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle,
units::meter_t leftDistance, units::meter_t rightDistance,
const Pose3d& initialPose)
: DifferentialDrivePoseEstimator3d{
kinematics, gyroAngle, leftDistance,
rightDistance, initialPose, {0.02, 0.02, 0.02, 0.01},
{0.1, 0.1, 0.1, 0.1}} {}
DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d(
DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle,
units::meter_t leftDistance, units::meter_t rightDistance,
const Pose3d& initialPose, const wpi::array<double, 4>& stateStdDevs,
const wpi::array<double, 4>& visionMeasurementStdDevs)
: PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs,
visionMeasurementStdDevs),
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {
ResetPose(initialPose);
}

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

@@ -24,8 +24,8 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
: PoseEstimator(kinematics, m_odometryImpl, stateStdDevs,
visionMeasurementStdDevs),
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {
ResetPose(m_odometryImpl.GetPose());
ResetPose(initialPose);
}

32
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp Normal file
View File

@@ -0,0 +1,32 @@
// 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.
#include "frc/estimator/MecanumDrivePoseEstimator3d.h"
#include <wpi/timestamp.h>
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "wpimath/MathShared.h"
using namespace frc;
frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d(
MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose)
: MecanumDrivePoseEstimator3d{
kinematics, gyroAngle,
wheelPositions, initialPose,
{0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}} {}
frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d(
MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose,
const wpi::array<double, 4>& stateStdDevs,
const wpi::array<double, 4>& visionMeasurementStdDevs)
: PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs,
visionMeasurementStdDevs),
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {
ResetPose(initialPose);
}

12
wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator3d.cpp Normal file
View File

@@ -0,0 +1,12 @@
// 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.
#include "frc/estimator/SwerveDrivePoseEstimator3d.h"
namespace frc {
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
SwerveDrivePoseEstimator3d<4>;
} // namespace frc

5
wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
View File

@@ -11,9 +11,8 @@ using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& initialPose)
: Odometry<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>(
m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
initialPose) {
: Odometry(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
initialPose) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
}

18
wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp Normal file
View File

@@ -0,0 +1,18 @@
// 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.
#include "frc/kinematics/DifferentialDriveOdometry3d.h"
#include "wpimath/MathShared.h"
using namespace frc;
DifferentialDriveOdometry3d::DifferentialDriveOdometry3d(
const Rotation3d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose3d& initialPose)
: Odometry3d(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
initialPose) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
}

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

@@ -11,8 +11,7 @@ using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
: Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
: Odometry(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);

18
wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp Normal file
View File

@@ -0,0 +1,18 @@
// 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.
#include "frc/kinematics/MecanumDriveOdometry3d.h"
#include "wpimath/MathShared.h"
using namespace frc;
MecanumDriveOdometry3d::MecanumDriveOdometry3d(
MecanumDriveKinematics kinematics, const Rotation3d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose)
: Odometry3d(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
}

12
wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry3d.cpp Normal file
View File

@@ -0,0 +1,12 @@
// 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.
#include "frc/kinematics/SwerveDriveOdometry3d.h"
namespace frc {
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
SwerveDriveOdometry3d<4>;
} // namespace frc