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

[wpilib] Add X and Y component getters to Pose2d and Transform2d (#2563)

Browse Source 
pose.Translation().X() and pose.Translation.Y() are common operations,
so shortening them to pose.X() and pose.Y() would be convenient.

Java uses the getX() convention so that is used instead of X() for Java.
This commit is contained in:
Tyler Veness
2020-07-02 18:09:36 -07:00
committed by GitHub
parent 5ccc98bc14
commit 2a0f79b90f
39 changed files with 228 additions and 192 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
View File

@@ -48,10 +48,8 @@ TEST(RamseteControllerTest, ReachesReference) {
}
auto& endPose = trajectory.States().back().pose;
EXPECT_NEAR_UNITS(endPose.Translation().X(), robotPose.Translation().X(),
kTolerance);
EXPECT_NEAR_UNITS(endPose.Translation().Y(), robotPose.Translation().Y(),
kTolerance);
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
EXPECT_NEAR_UNITS(boundRadians(endPose.Rotation().Radians() -
robotPose.Rotation().Radians()),
0_rad, kAngularTolerance);

18
wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -20,10 +20,8 @@ TEST(Pose2dTest, TransformBy) {
const auto transformed = initial + transform;
EXPECT_NEAR(transformed.Translation().X().to<double>(),
1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Translation().Y().to<double>(),
2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.X().to<double>(), 1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Y().to<double>(), 2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
}
@@ -33,10 +31,9 @@ TEST(Pose2dTest, RelativeTo) {
const auto finalRelativeToInitial = final.RelativeTo(initial);
EXPECT_NEAR(finalRelativeToInitial.Translation().X().to<double>(),
5.0 * std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Translation().Y().to<double>(), 0.0,
EXPECT_NEAR(finalRelativeToInitial.X().to<double>(), 5.0 * std::sqrt(2.0),
kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
kEpsilon);
}
@@ -59,8 +56,7 @@ TEST(Pose2dTest, Minus) {
const auto transform = final - initial;
EXPECT_NEAR(transform.Translation().X().to<double>(), 5.0 * std::sqrt(2.0),
kEpsilon);
EXPECT_NEAR(transform.Translation().Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(transform.X().to<double>(), 5.0 * std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transform.Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
}

8
wpilibc/src/test/native/cpp/geometry/Transform2dTest.cpp
View File

@@ -24,10 +24,10 @@ TEST(Transform2dTest, Inverse) {
auto transformed = initial + transform;
auto untransformed = transformed + transform.Inverse();
EXPECT_NEAR(initial.Translation().X().to<double>(),
untransformed.Translation().X().to<double>(), kEpsilon);
EXPECT_NEAR(initial.Translation().Y().to<double>(),
untransformed.Translation().Y().to<double>(), kEpsilon);
EXPECT_NEAR(initial.X().to<double>(), untransformed.X().to<double>(),
kEpsilon);
EXPECT_NEAR(initial.Y().to<double>(), untransformed.Y().to<double>(),
kEpsilon);
EXPECT_NEAR(initial.Rotation().Degrees().to<double>(),
untransformed.Rotation().Degrees().to<double>(), kEpsilon);
}

14
wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -20,8 +20,8 @@ TEST(Twist2dTest, Straight) {
const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
const auto straightPose = Pose2d().Exp(straight);
EXPECT_NEAR(straightPose.Translation().X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Translation().Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
}
@@ -30,8 +30,8 @@ TEST(Twist2dTest, QuarterCircle) {
units::radian_t(wpi::math::pi / 2.0)};
const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
EXPECT_NEAR(quarterCirclePose.Translation().X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Translation().Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
kEpsilon);
}
@@ -40,8 +40,8 @@ TEST(Twist2dTest, DiagonalNoDtheta) {
const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
const auto diagonalPose = Pose2d().Exp(diagonal);
EXPECT_NEAR(diagonalPose.Translation().X().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Translation().Y().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.X().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Y().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
}

6
wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -21,7 +21,7 @@ TEST(DifferentialDriveOdometry, EncoderDistances) {
const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
units::meter_t(5 * wpi::math::pi));
EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
}

18
wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -29,8 +29,8 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
EXPECT_NEAR(secondPose.Translation().X().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Translation().Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.X().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
}
@@ -41,8 +41,8 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
}
@@ -53,8 +53,8 @@ TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.Translation().X().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.Translation().Y().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.X().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
}
@@ -66,7 +66,7 @@ TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
}

14
wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -34,8 +34,8 @@ TEST_F(SwerveDriveOdometryTest, TwoIterations) {
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
state, state);
EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
}
@@ -52,8 +52,8 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
auto pose =
m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
EXPECT_NEAR(12.0, pose.Translation().X().to<double>(), kEpsilon);
EXPECT_NEAR(12.0, pose.Translation().Y().to<double>(), kEpsilon);
EXPECT_NEAR(12.0, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
}
@@ -68,7 +68,7 @@ TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
state, state);
EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
}

12
wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
View File

@@ -66,10 +66,8 @@ class CubicHermiteSplineTest : public ::testing::Test {
}
// Check first point.
EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
a.Translation().X().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
a.Translation().Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
a.Rotation().Radians().to<double>(), 1E-9);
@@ -90,10 +88,8 @@ class CubicHermiteSplineTest : public ::testing::Test {
EXPECT_TRUE(interiorsGood);
// Check last point.
EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
b.Translation().X().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
b.Translation().Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
b.Rotation().Radians().to<double>(), 1E-9);

12
wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
View File

@@ -54,18 +54,14 @@ class QuinticHermiteSplineTest : public ::testing::Test {
}
// Check first point.
EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
a.Translation().X().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
a.Translation().Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
a.Rotation().Radians().to<double>(), 1E-9);
// Check last point.
EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
b.Translation().X().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
b.Translation().Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
b.Rotation().Radians().to<double>(), 1E-9);

16
wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -24,10 +24,8 @@ void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
auto a = a2.RelativeTo(a1);
auto b = b2.RelativeTo(b1);
EXPECT_NEAR(a.Translation().X().to<double>(),
b.Translation().X().to<double>(), 1E-9);
EXPECT_NEAR(a.Translation().Y().to<double>(),
b.Translation().Y().to<double>(), 1E-9);
EXPECT_NEAR(a.X().to<double>(), b.X().to<double>(), 1E-9);
EXPECT_NEAR(a.Y().to<double>(), b.Y().to<double>(), 1E-9);
EXPECT_NEAR(a.Rotation().Radians().to<double>(),
b.Rotation().Radians().to<double>(), 1E-9);
}
@@ -44,8 +42,8 @@ TEST(TrajectoryTransforms, TransformBy) {
auto firstPose = transformedTrajectory.Sample(0_s).pose;
EXPECT_NEAR(firstPose.Translation().X().to<double>(), 1.0, 1E-9);
EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 2.0, 1E-9);
EXPECT_NEAR(firstPose.X().to<double>(), 1.0, 1E-9);
EXPECT_NEAR(firstPose.Y().to<double>(), 2.0, 1E-9);
EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
@@ -62,8 +60,8 @@ TEST(TrajectoryTransforms, RelativeTo) {
auto firstPose = transformedTrajectory.Sample(0_s).pose;
EXPECT_NEAR(firstPose.Translation().X().to<double>(), 0, 1E-9);
EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 0, 1E-9);
EXPECT_NEAR(firstPose.X().to<double>(), 0, 1E-9);
EXPECT_NEAR(firstPose.Y().to<double>(), 0, 1E-9);
EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());