mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-05 03:21:42 +00:00
9e1258440b
Documents the extrinsic vs intrinsic semantics of `plus()` and `minus()`. (`rotateBy()` was documented in [a previous PR](https://github.com/wpilibsuite/allwpilib/pull/5508)) Fixes usage of `plus()` and `minus()` in `Rotation3d.interpolate()`. (Fixes #8523) Fixes incorrect usages of `plus()`, `minus()`, and `rotateBy()` throughout `Odometry3d`. Adds explanatory comments for some `plus()`, `minus()`, and `rotateBy()` operations. Fixes `TimeInterpolatableBuffer` not using twists for `Pose3d` (this was just because I happened to notice it, it isn't really related to the PR) To check all of our usages of `plus()`, `minus()`, and `rotateBy()`, I marked them as deprecated, checked compile errors from `./gradlew compileJava`, and then undeprecated them. You can see all of the spots that showed up (at least on the Java side) by viewing the diff for 241109c. I wanted to present this alternative to #8526 because the change has its own quirks, there's little time before kickoff, and there would be no code-side warning to teams (and mentors) already used to the current behavior.
56 lines
2.1 KiB
C++
56 lines
2.1 KiB
C++
// 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 <numbers>
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
|
#include "frc/kinematics/DifferentialDriveOdometry3d.h"
|
|
|
|
static constexpr double kEpsilon = 1E-9;
|
|
|
|
using namespace frc;
|
|
|
|
TEST(DifferentialDriveOdometry3dTest, Initialize) {
|
|
DifferentialDriveOdometry3d odometry{
|
|
frc::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m,
|
|
frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}};
|
|
|
|
const frc::Pose3d& pose = odometry.GetPose();
|
|
|
|
EXPECT_NEAR(pose.X().value(), 1, kEpsilon);
|
|
EXPECT_NEAR(pose.Y().value(), 2, kEpsilon);
|
|
EXPECT_NEAR(pose.Z().value(), 0, kEpsilon);
|
|
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, kEpsilon);
|
|
}
|
|
|
|
TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
|
|
DifferentialDriveOdometry3d odometry{frc::Rotation3d{0_deg, 0_deg, 45_deg},
|
|
0_m, 0_m};
|
|
|
|
const auto& pose = odometry.Update(frc::Rotation3d{0_deg, 0_deg, 135_deg},
|
|
0_m, units::meter_t{5 * std::numbers::pi});
|
|
|
|
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
|
|
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
|
|
EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
|
|
EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon);
|
|
}
|
|
|
|
TEST(DifferentialDriveOdometry3dTest, GyroOffset) {
|
|
DifferentialDriveOdometry3d odometry{
|
|
frc::Rotation3d{0_deg, 5_deg, 0_deg}, 0_m, 0_m,
|
|
frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}}};
|
|
const auto& pose =
|
|
odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg}, 0_m, 0_m);
|
|
|
|
EXPECT_NEAR(pose.X().value(), 0.0, kEpsilon);
|
|
EXPECT_NEAR(pose.Y().value(), 0.0, kEpsilon);
|
|
EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
|
|
EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, kEpsilon);
|
|
EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, kEpsilon);
|
|
EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, kEpsilon);
|
|
}
|