diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java index 7e914fcc7a..b112a523e8 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -260,12 +260,12 @@ public class MecanumControllerCommand extends CommandBase { var poseError = desiredPose.relativeTo(m_pose.get()); double targetXVel = m_xController.calculate( - m_pose.get().getTranslation().getX(), - desiredPose.getTranslation().getX()); + m_pose.get().getX(), + desiredPose.getX()); double targetYVel = m_yController.calculate( - m_pose.get().getTranslation().getY(), - desiredPose.getTranslation().getY()); + m_pose.get().getY(), + desiredPose.getY()); // The robot will go to the desired rotation of the final pose in the trajectory, // not following the poses at individual states. diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java index 453fcfda38..7ea198392a 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java @@ -120,12 +120,12 @@ public class SwerveControllerCommand extends CommandBase { var poseError = desiredPose.relativeTo(m_pose.get()); double targetXVel = m_xController.calculate( - m_pose.get().getTranslation().getX(), - desiredPose.getTranslation().getX()); + m_pose.get().getX(), + desiredPose.getX()); double targetYVel = m_yController.calculate( - m_pose.get().getTranslation().getY(), - desiredPose.getTranslation().getY()); + m_pose.get().getY(), + desiredPose.getY()); // The robot will go to the desired rotation of the final pose in the trajectory, // not following the poses at individual states. diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index 8edc935e6a..fbc3c7355d 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -169,12 +169,10 @@ void MecanumControllerCommand::Execute() { auto m_poseError = m_desiredPose.RelativeTo(m_pose()); - auto targetXVel = meters_per_second_t( - m_xController->Calculate((m_pose().Translation().X().to()), - (m_desiredPose.Translation().X().to()))); - auto targetYVel = meters_per_second_t( - m_yController->Calculate((m_pose().Translation().Y().to()), - (m_desiredPose.Translation().Y().to()))); + auto targetXVel = meters_per_second_t(m_xController->Calculate( + (m_pose().X().to()), (m_desiredPose.X().to()))); + auto targetYVel = meters_per_second_t(m_yController->Calculate( + (m_pose().Y().to()), (m_desiredPose.Y().to()))); // Profiled PID Controller only takes meters as setpoint and measurement // The robot will go to the desired rotation of the final pose in the diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc index da3daf568f..9c047ad482 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc @@ -68,12 +68,12 @@ void SwerveControllerCommand::Execute() { auto m_poseError = m_desiredPose.RelativeTo(m_pose()); - auto targetXVel = units::meters_per_second_t(m_xController->Calculate( - (m_pose().Translation().X().template to()), - (m_desiredPose.Translation().X().template to()))); - auto targetYVel = units::meters_per_second_t(m_yController->Calculate( - (m_pose().Translation().Y().template to()), - (m_desiredPose.Translation().Y().template to()))); + auto targetXVel = units::meters_per_second_t( + m_xController->Calculate((m_pose().X().template to()), + (m_desiredPose.X().template to()))); + auto targetYVel = units::meters_per_second_t( + m_yController->Calculate((m_pose().Y().template to()), + (m_desiredPose.Y().template to()))); // Profiled PID Controller only takes meters as setpoint and measurement // The robot will go to the desired rotation of the final pose in the diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java index 2ac2129b68..9467c62883 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java @@ -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. */ @@ -107,10 +107,10 @@ class MecanumControllerCommandTest { command.end(true); assertAll( - () -> assertEquals(endState.poseMeters.getTranslation().getX(), - getRobotPose().getTranslation().getX(), kxTolerance), - () -> assertEquals(endState.poseMeters.getTranslation().getY(), - getRobotPose().getTranslation().getY(), kyTolerance), + () -> assertEquals(endState.poseMeters.getX(), + getRobotPose().getX(), kxTolerance), + () -> assertEquals(endState.poseMeters.getY(), + getRobotPose().getY(), kyTolerance), () -> assertEquals(endState.poseMeters.getRotation().getRadians(), getRobotPose().getRotation().getRadians(), kAngularTolerance) ); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java index 5209199cf9..fcddd5eba2 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java @@ -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. */ @@ -100,10 +100,10 @@ class SwerveControllerCommandTest { command.end(true); assertAll( - () -> assertEquals(endState.poseMeters.getTranslation().getX(), - getRobotPose().getTranslation().getX(), kxTolerance), - () -> assertEquals(endState.poseMeters.getTranslation().getY(), - getRobotPose().getTranslation().getY(), kyTolerance), + () -> assertEquals(endState.poseMeters.getX(), + getRobotPose().getX(), kxTolerance), + () -> assertEquals(endState.poseMeters.getY(), + getRobotPose().getY(), kyTolerance), () -> assertEquals(endState.poseMeters.getRotation().getRadians(), getRobotPose().getRotation().getRadians(), kAngularTolerance) ); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp index 4eae88017b..abfc84bee8 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp @@ -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. */ @@ -107,10 +107,8 @@ TEST_F(MecanumControllerCommandTest, ReachesReference) { m_timer.Stop(); command.End(false); - EXPECT_NEAR_UNITS(endState.pose.Translation().X(), - getRobotPose().Translation().X(), kxTolerance); - EXPECT_NEAR_UNITS(endState.pose.Translation().Y(), - getRobotPose().Translation().Y(), kyTolerance); + EXPECT_NEAR_UNITS(endState.pose.X(), getRobotPose().X(), kxTolerance); + EXPECT_NEAR_UNITS(endState.pose.Y(), getRobotPose().Y(), kyTolerance); EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(), getRobotPose().Rotation().Radians(), kAngularTolerance); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp index fa0838d99e..83ba26f979 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp @@ -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. */ @@ -97,10 +97,8 @@ TEST_F(SwerveControllerCommandTest, ReachesReference) { m_timer.Stop(); command.End(false); - EXPECT_NEAR_UNITS(endState.pose.Translation().X(), - getRobotPose().Translation().X(), kxTolerance); - EXPECT_NEAR_UNITS(endState.pose.Translation().Y(), - getRobotPose().Translation().Y(), kyTolerance); + EXPECT_NEAR_UNITS(endState.pose.X(), getRobotPose().X(), kxTolerance); + EXPECT_NEAR_UNITS(endState.pose.Y(), getRobotPose().Y(), kyTolerance); EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(), getRobotPose().Rotation().Radians(), kAngularTolerance); } diff --git a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp index 466429e86a..aad6937907 100644 --- a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp +++ b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp @@ -54,8 +54,8 @@ ChassisSpeeds RamseteController::Calculate( m_poseError = poseRef.RelativeTo(currentPose); // Aliases for equation readability - double eX = m_poseError.Translation().X().to(); - double eY = m_poseError.Translation().Y().to(); + double eX = m_poseError.X().to(); + double eY = m_poseError.Y().to(); double eTheta = m_poseError.Rotation().Radians().to(); double vRef = linearVelocityRef.to(); double omegaRef = angularVelocityRef.to(); diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp index 3d71073137..9ddfb352cb 100644 --- a/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp @@ -48,9 +48,9 @@ bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) const { // If the inequality is satisfied, then it is inside the ellipse; otherwise // it is outside the ellipse. // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons. - return units::math::pow<2>(pose.Translation().X() - m_center.X()) * + return units::math::pow<2>(pose.X() - m_center.X()) * units::math::pow<2>(m_radii.Y()) + - units::math::pow<2>(pose.Translation().Y() - m_center.Y()) * + units::math::pow<2>(pose.Y() - m_center.Y()) * units::math::pow<2>(m_radii.X()) <= units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y()); } diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp index 7d76c70397..cf48381dd2 100644 --- a/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp @@ -39,8 +39,6 @@ TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration( } bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) const { - return pose.Translation().X() >= m_bottomLeftPoint.X() && - pose.Translation().X() <= m_topRightPoint.X() && - pose.Translation().Y() >= m_bottomLeftPoint.Y() && - pose.Translation().Y() <= m_topRightPoint.Y(); + return pose.X() >= m_bottomLeftPoint.X() && pose.X() <= m_topRightPoint.X() && + pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y(); } diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h index 46328a0e60..76eeb90a8d 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h @@ -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. */ @@ -103,6 +103,20 @@ class Pose2d { */ const Translation2d& Translation() const { return m_translation; } + /** + * Returns the X component of the pose's translation. + * + * @return The x component of the pose's translation. + */ + units::meter_t X() const { return m_translation.X(); } + + /** + * Returns the Y component of the pose's translation. + * + * @return The y component of the pose's translation. + */ + units::meter_t Y() const { return m_translation.Y(); } + /** * Returns the underlying rotation. * diff --git a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h index d77dd084a8..8f054138fa 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h @@ -46,6 +46,20 @@ class Transform2d { */ const Translation2d& Translation() const { return m_translation; } + /** + * Returns the X component of the transformation's translation. + * + * @return The x component of the transformation's translation. + */ + units::meter_t X() const { return m_translation.X(); } + + /** + * Returns the Y component of the transformation's translation. + * + * @return The y component of the transformation's translation. + */ + units::meter_t Y() const { return m_translation.Y(); } + /** * Returns the rotational component of the transformation. * diff --git a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h index 4ed1cf52f1..7a12542582 100644 --- a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h +++ b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h @@ -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. */ @@ -81,18 +81,14 @@ class SplineHelper { private: static Spline<3>::ControlVector CubicControlVector(double scalar, const Pose2d& point) { - return { - {point.Translation().X().to(), scalar * point.Rotation().Cos()}, - {point.Translation().Y().to(), - scalar * point.Rotation().Sin()}}; + return {{point.X().to(), scalar * point.Rotation().Cos()}, + {point.Y().to(), scalar * point.Rotation().Sin()}}; } static Spline<5>::ControlVector QuinticControlVector(double scalar, const Pose2d& point) { - return {{point.Translation().X().to(), - scalar * point.Rotation().Cos(), 0.0}, - {point.Translation().Y().to(), - scalar * point.Rotation().Sin(), 0.0}}; + return {{point.X().to(), scalar * point.Rotation().Cos(), 0.0}, + {point.Y().to(), scalar * point.Rotation().Sin(), 0.0}}; } /** diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp index 7cf84fe6fe..f7e9f2c754 100644 --- a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -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); diff --git a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp index 8b5f6745f3..620c9d38ab 100644 --- a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -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(), - 1 + 5 / std::sqrt(2.0), kEpsilon); - EXPECT_NEAR(transformed.Translation().Y().to(), - 2 + 5 / std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(transformed.X().to(), 1 + 5 / std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(transformed.Y().to(), 2 + 5 / std::sqrt(2.0), kEpsilon); EXPECT_NEAR(transformed.Rotation().Degrees().to(), 50.0, kEpsilon); } @@ -33,10 +31,9 @@ TEST(Pose2dTest, RelativeTo) { const auto finalRelativeToInitial = final.RelativeTo(initial); - EXPECT_NEAR(finalRelativeToInitial.Translation().X().to(), - 5.0 * std::sqrt(2.0), kEpsilon); - EXPECT_NEAR(finalRelativeToInitial.Translation().Y().to(), 0.0, + EXPECT_NEAR(finalRelativeToInitial.X().to(), 5.0 * std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(finalRelativeToInitial.Y().to(), 0.0, kEpsilon); EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to(), 0.0, kEpsilon); } @@ -59,8 +56,7 @@ TEST(Pose2dTest, Minus) { const auto transform = final - initial; - EXPECT_NEAR(transform.Translation().X().to(), 5.0 * std::sqrt(2.0), - kEpsilon); - EXPECT_NEAR(transform.Translation().Y().to(), 0.0, kEpsilon); + EXPECT_NEAR(transform.X().to(), 5.0 * std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(transform.Y().to(), 0.0, kEpsilon); EXPECT_NEAR(transform.Rotation().Degrees().to(), 0.0, kEpsilon); } diff --git a/wpilibc/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Transform2dTest.cpp index a0cfb4945a..b302fad6af 100644 --- a/wpilibc/src/test/native/cpp/geometry/Transform2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Transform2dTest.cpp @@ -24,10 +24,10 @@ TEST(Transform2dTest, Inverse) { auto transformed = initial + transform; auto untransformed = transformed + transform.Inverse(); - EXPECT_NEAR(initial.Translation().X().to(), - untransformed.Translation().X().to(), kEpsilon); - EXPECT_NEAR(initial.Translation().Y().to(), - untransformed.Translation().Y().to(), kEpsilon); + EXPECT_NEAR(initial.X().to(), untransformed.X().to(), + kEpsilon); + EXPECT_NEAR(initial.Y().to(), untransformed.Y().to(), + kEpsilon); EXPECT_NEAR(initial.Rotation().Degrees().to(), untransformed.Rotation().Degrees().to(), kEpsilon); } diff --git a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp index faf36d9f21..4766bd4854 100644 --- a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp @@ -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(), 5.0, kEpsilon); - EXPECT_NEAR(straightPose.Translation().Y().to(), 0.0, kEpsilon); + EXPECT_NEAR(straightPose.X().to(), 5.0, kEpsilon); + EXPECT_NEAR(straightPose.Y().to(), 0.0, kEpsilon); EXPECT_NEAR(straightPose.Rotation().Radians().to(), 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(), 5.0, kEpsilon); - EXPECT_NEAR(quarterCirclePose.Translation().Y().to(), 5.0, kEpsilon); + EXPECT_NEAR(quarterCirclePose.X().to(), 5.0, kEpsilon); + EXPECT_NEAR(quarterCirclePose.Y().to(), 5.0, kEpsilon); EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to(), 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(), 2.0, kEpsilon); - EXPECT_NEAR(diagonalPose.Translation().Y().to(), 2.0, kEpsilon); + EXPECT_NEAR(diagonalPose.X().to(), 2.0, kEpsilon); + EXPECT_NEAR(diagonalPose.Y().to(), 2.0, kEpsilon); EXPECT_NEAR(diagonalPose.Rotation().Degrees().to(), 0.0, kEpsilon); } diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp index 8acaf92314..89d65ea244 100644 --- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp +++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp @@ -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(), 5.0, kEpsilon); - EXPECT_NEAR(pose.Translation().Y().to(), 5.0, kEpsilon); + EXPECT_NEAR(pose.X().to(), 5.0, kEpsilon); + EXPECT_NEAR(pose.Y().to(), 5.0, kEpsilon); EXPECT_NEAR(pose.Rotation().Degrees().to(), 90.0, kEpsilon); } diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp index 5d990bfad3..cb85ec7e99 100644 --- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp +++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp @@ -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(), 0.0, 0.01); - EXPECT_NEAR(secondPose.Translation().Y().to(), 0.0, 0.01); + EXPECT_NEAR(secondPose.X().to(), 0.0, 0.01); + EXPECT_NEAR(secondPose.Y().to(), 0.0, 0.01); EXPECT_NEAR(secondPose.Rotation().Radians().to(), 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(), 0.5, 0.01); - EXPECT_NEAR(pose.Translation().Y().to(), 0.0, 0.01); + EXPECT_NEAR(pose.X().to(), 0.5, 0.01); + EXPECT_NEAR(pose.Y().to(), 0.0, 0.01); EXPECT_NEAR(pose.Rotation().Radians().to(), 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(), 12, 0.01); - EXPECT_NEAR(pose.Translation().Y().to(), 12, 0.01); + EXPECT_NEAR(pose.X().to(), 12, 0.01); + EXPECT_NEAR(pose.Y().to(), 12, 0.01); EXPECT_NEAR(pose.Rotation().Degrees().to(), 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(), 0.5, 0.01); - EXPECT_NEAR(pose.Translation().Y().to(), 0.0, 0.01); + EXPECT_NEAR(pose.X().to(), 0.5, 0.01); + EXPECT_NEAR(pose.Y().to(), 0.0, 0.01); EXPECT_NEAR(pose.Rotation().Radians().to(), 0.0, 0.01); } diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp index d0399dcdd9..40207a1952 100644 --- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp +++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp @@ -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(), kEpsilon); - EXPECT_NEAR(0.0, pose.Translation().Y().to(), kEpsilon); + EXPECT_NEAR(0.5, pose.X().to(), kEpsilon); + EXPECT_NEAR(0.0, pose.Y().to(), kEpsilon); EXPECT_NEAR(0.0, pose.Rotation().Degrees().to(), 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(), kEpsilon); - EXPECT_NEAR(12.0, pose.Translation().Y().to(), kEpsilon); + EXPECT_NEAR(12.0, pose.X().to(), kEpsilon); + EXPECT_NEAR(12.0, pose.Y().to(), kEpsilon); EXPECT_NEAR(90.0, pose.Rotation().Degrees().to(), 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(), kEpsilon); - EXPECT_NEAR(0.0, pose.Translation().Y().to(), kEpsilon); + EXPECT_NEAR(0.5, pose.X().to(), kEpsilon); + EXPECT_NEAR(0.0, pose.Y().to(), kEpsilon); EXPECT_NEAR(0.0, pose.Rotation().Degrees().to(), kEpsilon); } diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp index 5e8cb8de59..d59ed014d8 100644 --- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp +++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp @@ -66,10 +66,8 @@ class CubicHermiteSplineTest : public ::testing::Test { } // Check first point. - EXPECT_NEAR(poses.front().first.Translation().X().to(), - a.Translation().X().to(), 1E-9); - EXPECT_NEAR(poses.front().first.Translation().Y().to(), - a.Translation().Y().to(), 1E-9); + EXPECT_NEAR(poses.front().first.X().to(), a.X().to(), 1E-9); + EXPECT_NEAR(poses.front().first.Y().to(), a.Y().to(), 1E-9); EXPECT_NEAR(poses.front().first.Rotation().Radians().to(), a.Rotation().Radians().to(), 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(), - b.Translation().X().to(), 1E-9); - EXPECT_NEAR(poses.back().first.Translation().Y().to(), - b.Translation().Y().to(), 1E-9); + EXPECT_NEAR(poses.back().first.X().to(), b.X().to(), 1E-9); + EXPECT_NEAR(poses.back().first.Y().to(), b.Y().to(), 1E-9); EXPECT_NEAR(poses.back().first.Rotation().Radians().to(), b.Rotation().Radians().to(), 1E-9); diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp index 12445e72d9..56e3b2214b 100644 --- a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp +++ b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp @@ -54,18 +54,14 @@ class QuinticHermiteSplineTest : public ::testing::Test { } // Check first point. - EXPECT_NEAR(poses.front().first.Translation().X().to(), - a.Translation().X().to(), 1E-9); - EXPECT_NEAR(poses.front().first.Translation().Y().to(), - a.Translation().Y().to(), 1E-9); + EXPECT_NEAR(poses.front().first.X().to(), a.X().to(), 1E-9); + EXPECT_NEAR(poses.front().first.Y().to(), a.Y().to(), 1E-9); EXPECT_NEAR(poses.front().first.Rotation().Radians().to(), a.Rotation().Radians().to(), 1E-9); // Check last point. - EXPECT_NEAR(poses.back().first.Translation().X().to(), - b.Translation().X().to(), 1E-9); - EXPECT_NEAR(poses.back().first.Translation().Y().to(), - b.Translation().Y().to(), 1E-9); + EXPECT_NEAR(poses.back().first.X().to(), b.X().to(), 1E-9); + EXPECT_NEAR(poses.back().first.Y().to(), b.Y().to(), 1E-9); EXPECT_NEAR(poses.back().first.Rotation().Radians().to(), b.Rotation().Radians().to(), 1E-9); diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp index af69b58c6e..349ff5cbd9 100644 --- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp +++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp @@ -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 statesA, auto a = a2.RelativeTo(a1); auto b = b2.RelativeTo(b1); - EXPECT_NEAR(a.Translation().X().to(), - b.Translation().X().to(), 1E-9); - EXPECT_NEAR(a.Translation().Y().to(), - b.Translation().Y().to(), 1E-9); + EXPECT_NEAR(a.X().to(), b.X().to(), 1E-9); + EXPECT_NEAR(a.Y().to(), b.Y().to(), 1E-9); EXPECT_NEAR(a.Rotation().Radians().to(), b.Rotation().Radians().to(), 1E-9); } @@ -44,8 +42,8 @@ TEST(TrajectoryTransforms, TransformBy) { auto firstPose = transformedTrajectory.Sample(0_s).pose; - EXPECT_NEAR(firstPose.Translation().X().to(), 1.0, 1E-9); - EXPECT_NEAR(firstPose.Translation().Y().to(), 2.0, 1E-9); + EXPECT_NEAR(firstPose.X().to(), 1.0, 1E-9); + EXPECT_NEAR(firstPose.Y().to(), 2.0, 1E-9); EXPECT_NEAR(firstPose.Rotation().Degrees().to(), 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(), 0, 1E-9); - EXPECT_NEAR(firstPose.Translation().Y().to(), 0, 1E-9); + EXPECT_NEAR(firstPose.X().to(), 0, 1E-9); + EXPECT_NEAR(firstPose.Y().to(), 0, 1E-9); EXPECT_NEAR(firstPose.Rotation().Degrees().to(), 0, 1E-9); TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States()); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java index ef2100adae..43a61bdbdd 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java @@ -119,8 +119,8 @@ public class RamseteController { m_poseError = poseRef.relativeTo(currentPose); // Aliases for equation readability - final double eX = m_poseError.getTranslation().getX(); - final double eY = m_poseError.getTranslation().getY(); + final double eX = m_poseError.getX(); + final double eY = m_poseError.getY(); final double eTheta = m_poseError.getRotation().getRadians(); final double vRef = linearVelocityRefMeters; final double omegaRef = angularVelocityRefRadiansPerSecond; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java index c6839e600c..e75983f00a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java @@ -96,6 +96,24 @@ public class Pose2d { return m_translation; } + /** + * Returns the X component of the pose's translation. + * + * @return The x component of the pose's translation. + */ + public double getX() { + return m_translation.getX(); + } + + /** + * Returns the Y component of the pose's translation. + * + * @return The y component of the pose's translation. + */ + public double getY() { + return m_translation.getY(); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java index ef82b92f33..16746d5160 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java @@ -70,6 +70,24 @@ public class Transform2d { return m_translation; } + /** + * Returns the X component of the transformation's translation. + * + * @return The x component of the transformation's translation. + */ + public double getX() { + return m_translation.getX(); + } + + /** + * Returns the Y component of the transformation's translation. + * + * @return The y component of the transformation's translation. + */ + public double getY() { + return m_translation.getY(); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java index e6ca75d03f..d270554625 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java @@ -263,15 +263,15 @@ public final class SplineHelper { private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) { return new Spline.ControlVector( - new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos()}, - new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin()} + new double[]{point.getX(), scalar * point.getRotation().getCos()}, + new double[]{point.getY(), scalar * point.getRotation().getSin()} ); } private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) { return new Spline.ControlVector( - new double[]{point.getTranslation().getX(), scalar * point.getRotation().getCos(), 0.0}, - new double[]{point.getTranslation().getY(), scalar * point.getRotation().getSin(), 0.0} + new double[]{point.getX(), scalar * point.getRotation().getCos(), 0.0}, + new double[]{point.getY(), scalar * point.getRotation().getSin(), 0.0} ); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java index 5ff2a7d073..eb9f7e7fb6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java @@ -72,9 +72,9 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint { // If the inequality is satisfied, then it is inside the ellipse; otherwise // it is outside the ellipse. // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons. - return Math.pow(robotPose.getTranslation().getX() - m_center.getX(), 2) + return Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.getY(), 2) - + Math.pow(robotPose.getTranslation().getY() - m_center.getY(), 2) + + Math.pow(robotPose.getY() - m_center.getY(), 2) * Math.pow(m_radii.getX(), 2) <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java index 4fa8e7d99b..c25c74c650 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java @@ -65,9 +65,9 @@ public class RectangularRegionConstraint implements TrajectoryConstraint { * @return Whether the robot pose is within the constraint region. */ public boolean isPoseInRegion(Pose2d robotPose) { - return robotPose.getTranslation().getX() >= m_bottomLeftPoint.getX() - && robotPose.getTranslation().getX() <= m_topRightPoint.getX() - && robotPose.getTranslation().getY() >= m_bottomLeftPoint.getY() - && robotPose.getTranslation().getY() <= m_topRightPoint.getY(); + return robotPose.getX() >= m_bottomLeftPoint.getX() + && robotPose.getX() <= m_topRightPoint.getX() + && robotPose.getY() >= m_bottomLeftPoint.getY() + && robotPose.getY() <= m_topRightPoint.getY(); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java index 2bef50d8d8..1b41479122 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java @@ -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. */ @@ -63,9 +63,9 @@ class RamseteControllerTest { // must be final or effectively final. final var finalRobotPose = robotPose; assertAll( - () -> assertEquals(endPose.getTranslation().getX(), finalRobotPose.getTranslation().getX(), + () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance), - () -> assertEquals(endPose.getTranslation().getY(), finalRobotPose.getTranslation().getY(), + () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance), () -> assertEquals(0.0, boundRadians(endPose.getRotation().getRadians() diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java index 066716e355..14bad1fb64 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java @@ -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. */ @@ -25,8 +25,8 @@ class Pose2dTest { var transformed = initial.plus(transformation); assertAll( - () -> assertEquals(transformed.getTranslation().getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon), - () -> assertEquals(transformed.getTranslation().getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon), + () -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon), + () -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon), () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon) ); } @@ -39,9 +39,9 @@ class Pose2dTest { var finalRelativeToInitial = last.relativeTo(initial); assertAll( - () -> assertEquals(finalRelativeToInitial.getTranslation().getX(), 5.0 * Math.sqrt(2.0), + () -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0), kEpsilon), - () -> assertEquals(finalRelativeToInitial.getTranslation().getY(), 0.0, kEpsilon), + () -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon), () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon) ); } @@ -67,8 +67,8 @@ class Pose2dTest { final var transform = last.minus(initial); assertAll( - () -> assertEquals(transform.getTranslation().getX(), 5.0 * Math.sqrt(2.0), kEpsilon), - () -> assertEquals(transform.getTranslation().getY(), 0.0, kEpsilon), + () -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon), + () -> assertEquals(transform.getY(), 0.0, kEpsilon), () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon) ); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java index fd1be7bb52..c375d22af6 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java @@ -25,9 +25,9 @@ class Transform2dTest { var untransformed = transformed.plus(transformation.inverse()); assertAll( - () -> assertEquals(initial.getTranslation().getX(), untransformed.getTranslation().getX(), + () -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon), - () -> assertEquals(initial.getTranslation().getY(), untransformed.getTranslation().getY(), + () -> assertEquals(initial.getY(), untransformed.getY(), kEpsilon), () -> assertEquals(initial.getRotation().getDegrees(), untransformed.getRotation().getDegrees(), kEpsilon) diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java index 903c43690d..18ea6d97b3 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java @@ -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. */ @@ -22,8 +22,8 @@ class Twist2dTest { var straightPose = new Pose2d().exp(straight); assertAll( - () -> assertEquals(straightPose.getTranslation().getX(), 5.0, kEpsilon), - () -> assertEquals(straightPose.getTranslation().getY(), 0.0, kEpsilon), + () -> assertEquals(straightPose.getX(), 5.0, kEpsilon), + () -> assertEquals(straightPose.getY(), 0.0, kEpsilon), () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon) ); } @@ -34,8 +34,8 @@ class Twist2dTest { var quarterCirclePose = new Pose2d().exp(quarterCircle); assertAll( - () -> assertEquals(quarterCirclePose.getTranslation().getX(), 5.0, kEpsilon), - () -> assertEquals(quarterCirclePose.getTranslation().getY(), 5.0, kEpsilon), + () -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon), + () -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon), () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon) ); } @@ -46,8 +46,8 @@ class Twist2dTest { var diagonalPose = new Pose2d().exp(diagonal); assertAll( - () -> assertEquals(diagonalPose.getTranslation().getX(), 2.0, kEpsilon), - () -> assertEquals(diagonalPose.getTranslation().getY(), 2.0, kEpsilon), + () -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon), + () -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon), () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon) ); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java index aabc455344..e6022de9b8 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java @@ -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. */ @@ -26,8 +26,8 @@ class DifferentialDriveOdometryTest { var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI); assertAll( - () -> assertEquals(pose.getTranslation().getX(), 5.0, kEpsilon), - () -> assertEquals(pose.getTranslation().getY(), 5.0, kEpsilon), + () -> assertEquals(pose.getX(), 5.0, kEpsilon), + () -> assertEquals(pose.getY(), 5.0, kEpsilon), () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon) ); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java index 4619bf0dd0..5ece28b1bb 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java @@ -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. */ @@ -37,8 +37,8 @@ class MecanumDriveOdometryTest { var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds); assertAll( - () -> assertEquals(secondPose.getTranslation().getX(), 0.0, 0.01), - () -> assertEquals(secondPose.getTranslation().getY(), 0.0, 0.01), + () -> assertEquals(secondPose.getX(), 0.0, 0.01), + () -> assertEquals(secondPose.getY(), 0.0, 0.01), () -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01) ); } @@ -52,8 +52,8 @@ class MecanumDriveOdometryTest { var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds); assertAll( - () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01), - () -> assertEquals(0, pose.getTranslation().getY(), 0.01), + () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01), + () -> assertEquals(0, pose.getY(), 0.01), () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01) ); } @@ -68,8 +68,8 @@ class MecanumDriveOdometryTest { final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds); assertAll( - () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01), - () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01), + () -> assertEquals(12.0, pose.getX(), 0.01), + () -> assertEquals(12.0, pose.getY(), 0.01), () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01) ); } @@ -85,8 +85,8 @@ class MecanumDriveOdometryTest { var pose = m_odometry.updateWithTime(1.0, gyro, speeds); assertAll( - () -> assertEquals(5.0, pose.getTranslation().getX(), 0.1), - () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1), + () -> assertEquals(5.0, pose.getX(), 0.1), + () -> assertEquals(0.00, pose.getY(), 0.1), () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1) ); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java index 85b832c14b..f1ee90748e 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java @@ -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. */ @@ -44,8 +44,8 @@ class SwerveDriveOdometryTest { var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds); assertAll( - () -> assertEquals(5.0 / 10.0, pose.getTranslation().getX(), 0.01), - () -> assertEquals(0, pose.getTranslation().getY(), 0.01), + () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01), + () -> assertEquals(0, pose.getY(), 0.01), () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01) ); } @@ -70,8 +70,8 @@ class SwerveDriveOdometryTest { final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds); assertAll( - () -> assertEquals(12.0, pose.getTranslation().getX(), 0.01), - () -> assertEquals(12.0, pose.getTranslation().getY(), 0.01), + () -> assertEquals(12.0, pose.getX(), 0.01), + () -> assertEquals(12.0, pose.getY(), 0.01), () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01) ); } @@ -87,8 +87,8 @@ class SwerveDriveOdometryTest { var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state); assertAll( - () -> assertEquals(1.0, pose.getTranslation().getX(), 0.1), - () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1), + () -> assertEquals(1.0, pose.getX(), 0.1), + () -> assertEquals(0.00, pose.getY(), 0.1), () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1) ); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java index 45a063822f..2aac93da08 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java @@ -71,10 +71,10 @@ class CubicHermiteSplineTest { // Check first point assertAll( - () -> assertEquals(a.getTranslation().getX(), - poses.get(0).poseMeters.getTranslation().getX(), 1E-9), - () -> assertEquals(a.getTranslation().getY(), - poses.get(0).poseMeters.getTranslation().getY(), 1E-9), + () -> assertEquals(a.getX(), + poses.get(0).poseMeters.getX(), 1E-9), + () -> assertEquals(a.getY(), + poses.get(0).poseMeters.getY(), 1E-9), () -> assertEquals(a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(), 1E-9) ); @@ -95,10 +95,10 @@ class CubicHermiteSplineTest { // Check last point assertAll( - () -> assertEquals(b.getTranslation().getX(), - poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9), - () -> assertEquals(b.getTranslation().getY(), - poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9), + () -> assertEquals(b.getX(), + poses.get(poses.size() - 1).poseMeters.getX(), 1E-9), + () -> assertEquals(b.getY(), + poses.get(poses.size() - 1).poseMeters.getY(), 1E-9), () -> assertEquals(b.getRotation().getRadians(), poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9) ); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java index 1d65922ddd..052bd0468d 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java @@ -57,19 +57,19 @@ class QuinticHermiteSplineTest { // Check first point assertAll( () -> assertEquals( - a.getTranslation().getX(), poses.get(0).poseMeters.getTranslation().getX(), 1E-9), + a.getX(), poses.get(0).poseMeters.getX(), 1E-9), () -> assertEquals( - a.getTranslation().getY(), poses.get(0).poseMeters.getTranslation().getY(), 1E-9), + a.getY(), poses.get(0).poseMeters.getY(), 1E-9), () -> assertEquals( a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)); // Check last point assertAll( - () -> assertEquals(b.getTranslation().getX(), poses.get(poses.size() - 1) - .poseMeters.getTranslation().getX(), 1E-9), - () -> assertEquals(b.getTranslation().getY(), poses.get(poses.size() - 1) - .poseMeters.getTranslation().getY(), 1E-9), + () -> assertEquals(b.getX(), poses.get(poses.size() - 1) + .poseMeters.getX(), 1E-9), + () -> assertEquals(b.getY(), poses.get(poses.size() - 1) + .poseMeters.getY(), 1E-9), () -> assertEquals(b.getRotation().getRadians(), poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)); }