Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +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

8
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View File

@@ -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.

8
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
View File

@@ -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.

10
wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
View File

@@ -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<double>()),
(m_desiredPose.Translation().X().to<double>())));
auto targetYVel = meters_per_second_t(
m_yController->Calculate((m_pose().Translation().Y().to<double>()),
(m_desiredPose.Translation().Y().to<double>())));
auto targetXVel = meters_per_second_t(m_xController->Calculate(
(m_pose().X().to<double>()), (m_desiredPose.X().to<double>())));
auto targetYVel = meters_per_second_t(m_yController->Calculate(
(m_pose().Y().to<double>()), (m_desiredPose.Y().to<double>())));
// Profiled PID Controller only takes meters as setpoint and measurement
// The robot will go to the desired rotation of the final pose in the

12
wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
View File

@@ -68,12 +68,12 @@ void SwerveControllerCommand<NumModules>::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<double>()),
(m_desiredPose.Translation().X().template to<double>())));
auto targetYVel = units::meters_per_second_t(m_yController->Calculate(
(m_pose().Translation().Y().template to<double>()),
(m_desiredPose.Translation().Y().template to<double>())));
auto targetXVel = units::meters_per_second_t(
m_xController->Calculate((m_pose().X().template to<double>()),
(m_desiredPose.X().template to<double>())));
auto targetYVel = units::meters_per_second_t(
m_yController->Calculate((m_pose().Y().template to<double>()),
(m_desiredPose.Y().template to<double>())));
// Profiled PID Controller only takes meters as setpoint and measurement
// The robot will go to the desired rotation of the final pose in the

10
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
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. */
@@ -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)
);

10
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
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. */
@@ -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)
);

8
wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.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. */
@@ -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);
}

8
wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.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. */
@@ -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);
}

4
wpilibc/src/main/native/cpp/controller/RamseteController.cpp
View File

@@ -54,8 +54,8 @@ ChassisSpeeds RamseteController::Calculate(
m_poseError = poseRef.RelativeTo(currentPose);
// Aliases for equation readability
double eX = m_poseError.Translation().X().to<double>();
double eY = m_poseError.Translation().Y().to<double>();
double eX = m_poseError.X().to<double>();
double eY = m_poseError.Y().to<double>();
double eTheta = m_poseError.Rotation().Radians().to<double>();
double vRef = linearVelocityRef.to<double>();
double omegaRef = angularVelocityRef.to<double>();

4
wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp
View File

@@ -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());
}

6
wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp
View File

@@ -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();
}

16
wpilibc/src/main/native/include/frc/geometry/Pose2d.h
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. */
@@ -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.
*

14
wpilibc/src/main/native/include/frc/geometry/Transform2d.h
View File

@@ -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.
*

14
wpilibc/src/main/native/include/frc/spline/SplineHelper.h
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. */
@@ -81,18 +81,14 @@ class SplineHelper {
private:
static Spline<3>::ControlVector CubicControlVector(double scalar,
const Pose2d& point) {
return {
{point.Translation().X().to<double>(), scalar * point.Rotation().Cos()},
{point.Translation().Y().to<double>(),
scalar * point.Rotation().Sin()}};
return {{point.X().to<double>(), scalar * point.Rotation().Cos()},
{point.Y().to<double>(), scalar * point.Rotation().Sin()}};
}
static Spline<5>::ControlVector QuinticControlVector(double scalar,
const Pose2d& point) {
return {{point.Translation().X().to<double>(),
scalar * point.Rotation().Cos(), 0.0},
{point.Translation().Y().to<double>(),
scalar * point.Rotation().Sin(), 0.0}};
return {{point.X().to<double>(), scalar * point.Rotation().Cos(), 0.0},
{point.Y().to<double>(), scalar * point.Rotation().Sin(), 0.0}};
}
/**

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());

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
View File

@@ -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;

18
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
View File

@@ -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.
*

18
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
View File

@@ -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.
*

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
View File

@@ -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}
);
}
}

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java
View File

@@ -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);
}
}

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java
View File

@@ -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();
}
}

6
wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
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. */
@@ -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()

14
wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
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. */
@@ -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)
);
}

4
wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java
View File

@@ -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)

14
wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
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. */
@@ -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)
);
}

6
wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
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. */
@@ -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)
);
}

18
wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
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. */
@@ -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)
);
}

14
wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
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. */
@@ -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)
);
}

16
wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
View File

@@ -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)
);

12
wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
View File

@@ -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));
}