diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java index e0647f20b8..7daac52c8f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java @@ -4,14 +4,14 @@ package edu.wpi.first.math.trajectory.constraint; +import edu.wpi.first.math.geometry.Ellipse2d; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; /** Enforces a particular constraint only within an elliptical region. */ public class EllipticalRegionConstraint implements TrajectoryConstraint { - private final Translation2d m_center; - private final Translation2d m_radii; + private final Ellipse2d m_ellipse; private final TrajectoryConstraint m_constraint; /** @@ -22,22 +22,33 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint { * @param yWidth The height of the ellipse in which to enforce the constraint. * @param rotation The rotation to apply to all radii around the origin. * @param constraint The constraint to enforce when the robot is within the region. + * @deprecated Use constructor taking Ellipse2d instead. */ + @Deprecated(since = "2025", forRemoval = true) public EllipticalRegionConstraint( Translation2d center, double xWidth, double yWidth, Rotation2d rotation, TrajectoryConstraint constraint) { - m_center = center; - m_radii = new Translation2d(xWidth / 2.0, yWidth / 2.0).rotateBy(rotation); + this(new Ellipse2d(new Pose2d(center, rotation), xWidth / 2.0, yWidth / 2.0), constraint); + } + + /** + * Constructs a new EllipticalRegionConstraint. + * + * @param ellipse The ellipse in which to enforce the constraint. + * @param constraint The constraint to enforce when the robot is within the region. + */ + public EllipticalRegionConstraint(Ellipse2d ellipse, TrajectoryConstraint constraint) { + m_ellipse = ellipse; m_constraint = constraint; } @Override public double getMaxVelocityMetersPerSecond( Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { - if (isPoseInRegion(poseMeters)) { + if (m_ellipse.contains(poseMeters.getTranslation())) { return m_constraint.getMaxVelocityMetersPerSecond( poseMeters, curvatureRadPerMeter, velocityMetersPerSecond); } else { @@ -48,34 +59,11 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint { @Override public MinMax getMinMaxAccelerationMetersPerSecondSq( Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { - if (isPoseInRegion(poseMeters)) { + if (m_ellipse.contains(poseMeters.getTranslation())) { return m_constraint.getMinMaxAccelerationMetersPerSecondSq( poseMeters, curvatureRadPerMeter, velocityMetersPerSecond); } else { return new MinMax(); } } - - /** - * Returns whether the specified robot pose is within the region that the constraint is enforced - * in. - * - * @param robotPose The robot pose. - * @return Whether the robot pose is within the constraint region. - */ - public boolean isPoseInRegion(Pose2d robotPose) { - // The region bounded by the ellipse is given by the equation: - // - // (x−h)²/Rx² + (y−k)²/Ry² ≤ 1 - // - // Multiply by Rx²Ry² for efficiency reasons: - // - // (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry² - // - // If the inequality is satisfied, then it is inside the ellipse; otherwise - // it is outside the ellipse. - return Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.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/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java index b29df5e9ae..6966471984 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java @@ -5,12 +5,12 @@ package edu.wpi.first.math.trajectory.constraint; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Translation2d; /** Enforces a particular constraint only within a rectangular region. */ public class RectangularRegionConstraint implements TrajectoryConstraint { - private final Translation2d m_bottomLeftPoint; - private final Translation2d m_topRightPoint; + private final Rectangle2d m_rectangle; private final TrajectoryConstraint m_constraint; /** @@ -21,18 +21,29 @@ public class RectangularRegionConstraint implements TrajectoryConstraint { * @param topRightPoint The top right point of the rectangular region in which to enforce the * constraint. * @param constraint The constraint to enforce when the robot is within the region. + * @deprecated Use constructor taking Rectangle2d instead. */ + @Deprecated(since = "2025", forRemoval = true) public RectangularRegionConstraint( Translation2d bottomLeftPoint, Translation2d topRightPoint, TrajectoryConstraint constraint) { - m_bottomLeftPoint = bottomLeftPoint; - m_topRightPoint = topRightPoint; + this(new Rectangle2d(bottomLeftPoint, topRightPoint), constraint); + } + + /** + * Constructs a new RectangularRegionConstraint. + * + * @param rectangle The rectangular region in which to enforce the constraint. + * @param constraint The constraint to enforce when the robot is within the region. + */ + public RectangularRegionConstraint(Rectangle2d rectangle, TrajectoryConstraint constraint) { + m_rectangle = rectangle; m_constraint = constraint; } @Override public double getMaxVelocityMetersPerSecond( Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { - if (isPoseInRegion(poseMeters)) { + if (m_rectangle.contains(poseMeters.getTranslation())) { return m_constraint.getMaxVelocityMetersPerSecond( poseMeters, curvatureRadPerMeter, velocityMetersPerSecond); } else { @@ -43,25 +54,11 @@ public class RectangularRegionConstraint implements TrajectoryConstraint { @Override public MinMax getMinMaxAccelerationMetersPerSecondSq( Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { - if (isPoseInRegion(poseMeters)) { + if (m_rectangle.contains(poseMeters.getTranslation())) { return m_constraint.getMinMaxAccelerationMetersPerSecondSq( poseMeters, curvatureRadPerMeter, velocityMetersPerSecond); } else { return new MinMax(); } } - - /** - * Returns whether the specified robot pose is within the region that the constraint is enforced - * in. - * - * @param robotPose The robot pose. - * @return Whether the robot pose is within the constraint region. - */ - public boolean isPoseInRegion(Pose2d robotPose) { - return robotPose.getX() >= m_bottomLeftPoint.getX() - && robotPose.getX() <= m_topRightPoint.getX() - && robotPose.getY() >= m_bottomLeftPoint.getY() - && robotPose.getY() <= m_topRightPoint.getY(); - } } diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h index 74f3c55cd5..62e2764bf0 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h @@ -7,12 +7,14 @@ #include #include +#include "frc/geometry/Ellipse2d.h" #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Translation2d.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" #include "units/length.h" namespace frc { + /** * Enforces a particular constraint only within an elliptical region. */ @@ -27,21 +29,31 @@ class EllipticalRegionConstraint : public TrajectoryConstraint { * @param yWidth The height of the ellipse in which to enforce the constraint. * @param rotation The rotation to apply to all radii around the origin. * @param constraint The constraint to enforce when the robot is within the - * region. + * region. + * @deprecated Use constructor taking Ellipse2d instead. */ + [[deprecated("Use constructor taking Ellipse2d instead.")]] EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth, const Rotation2d& rotation, const Constraint& constraint) - : m_center(center), - m_radii(xWidth / 2.0, yWidth / 2.0), - m_constraint(constraint) { - m_radii = m_radii.RotateBy(rotation); - } + : m_ellipse{Pose2d{center, rotation}, xWidth / 2.0, yWidth / 2.0}, + m_constraint(constraint) {} + + /** + * Constructs a new EllipticalRegionConstraint. + * + * @param ellipse The ellipse in which to enforce the constraint. + * @param constraint The constraint to enforce when the robot is within the + * region. + */ + EllipticalRegionConstraint(const Ellipse2d& ellipse, + const Constraint& constraint) + : m_ellipse{ellipse}, m_constraint{constraint} {} units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, units::meters_per_second_t velocity) const override { - if (IsPoseInRegion(pose)) { + if (m_ellipse.Contains(pose.Translation())) { return m_constraint.MaxVelocity(pose, curvature, velocity); } else { return units::meters_per_second_t{ @@ -51,41 +63,16 @@ class EllipticalRegionConstraint : public TrajectoryConstraint { MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, units::meters_per_second_t speed) const override { - if (IsPoseInRegion(pose)) { + if (m_ellipse.Contains(pose.Translation())) { return m_constraint.MinMaxAcceleration(pose, curvature, speed); } else { return {}; } } - /** - * Returns whether the specified robot pose is within the region that the - * constraint is enforced in. - * - * @param pose The robot pose. - * @return Whether the robot pose is within the constraint region. - */ - bool IsPoseInRegion(const Pose2d& pose) const { - // The region bounded by the ellipse is given by the equation: - // - // (x−h)²/Rx² + (y−k)²/Ry² ≤ 1 - // - // Multiply by Rx²Ry² for efficiency reasons: - // - // (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry² - // - // If the inequality is satisfied, then it is inside the ellipse; otherwise - // it is outside the ellipse. - return units::math::pow<2>(pose.X() - m_center.X()) * - units::math::pow<2>(m_radii.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()); - } - private: - Translation2d m_center; - Translation2d m_radii; + Ellipse2d m_ellipse; Constraint m_constraint; }; + } // namespace frc diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h index f3b364b114..c0075b9041 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h @@ -7,11 +7,12 @@ #include #include -#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Rectangle2d.h" #include "frc/geometry/Translation2d.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" namespace frc { + /** * Enforces a particular constraint only within a rectangular region. */ @@ -22,23 +23,34 @@ class RectangularRegionConstraint : public TrajectoryConstraint { * Constructs a new RectangularRegionConstraint. * * @param bottomLeftPoint The bottom left point of the rectangular region in - * which to enforce the constraint. + * which to enforce the constraint. * @param topRightPoint The top right point of the rectangular region in which - * to enforce the constraint. + * to enforce the constraint. * @param constraint The constraint to enforce when the robot is within the - * region. + * region. + * @deprecated Use constructor taking Rectangle2d instead. */ + [[deprecated("Use constructor taking Rectangle2d instead.")]] RectangularRegionConstraint(const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint, const Constraint& constraint) - : m_bottomLeftPoint(bottomLeftPoint), - m_topRightPoint(topRightPoint), - m_constraint(constraint) {} + : m_rectangle{bottomLeftPoint, topRightPoint}, m_constraint(constraint) {} + + /** + * Constructs a new RectangularRegionConstraint. + * + * @param rectangle The rectangular region in which to enforce the constraint. + * @param constraint The constraint to enforce when the robot is within the + * region. + */ + RectangularRegionConstraint(const Rectangle2d& rectangle, + const Constraint& constraint) + : m_rectangle{rectangle}, m_constraint{constraint} {} units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, units::meters_per_second_t velocity) const override { - if (IsPoseInRegion(pose)) { + if (m_rectangle.Contains(pose.Translation())) { return m_constraint.MaxVelocity(pose, curvature, velocity); } else { return units::meters_per_second_t{ @@ -48,29 +60,16 @@ class RectangularRegionConstraint : public TrajectoryConstraint { MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, units::meters_per_second_t speed) const override { - if (IsPoseInRegion(pose)) { + if (m_rectangle.Contains(pose.Translation())) { return m_constraint.MinMaxAcceleration(pose, curvature, speed); } else { return {}; } } - /** - * Returns whether the specified robot pose is within the region that the - * constraint is enforced in. - * - * @param pose The robot pose. - * @return Whether the robot pose is within the constraint region. - */ - bool IsPoseInRegion(const Pose2d& pose) const { - return pose.X() >= m_bottomLeftPoint.X() && - pose.X() <= m_topRightPoint.X() && - pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y(); - } - private: - Translation2d m_bottomLeftPoint; - Translation2d m_topRightPoint; + Rectangle2d m_rectangle; Constraint m_constraint; }; + } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java index 96f83b3ece..273b67a3f5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java @@ -4,12 +4,11 @@ package edu.wpi.first.math.trajectory; -import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.geometry.Ellipse2d; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; import edu.wpi.first.math.util.Units; @@ -19,27 +18,21 @@ import org.junit.jupiter.api.Test; class EllipticalRegionConstraintTest { @Test void testConstraint() { - // Create constraints double maxVelocity = Units.feetToMeters(3.0); - var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity); - var regionConstraint = - new EllipticalRegionConstraint( - new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(5.0)), - Units.feetToMeters(10.0), + var ellipse = + new Ellipse2d( + new Pose2d(Units.feetToMeters(5.0), Units.feetToMeters(2.5), Rotation2d.kPi), Units.feetToMeters(5.0), - Rotation2d.kPi, - maxVelocityConstraint); + Units.feetToMeters(2.5)); - // Get trajectory - var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint)); + var trajectory = + TrajectoryGeneratorTest.getTrajectory( + List.of( + new EllipticalRegionConstraint(ellipse, new MaxVelocityConstraint(maxVelocity)))); - // Iterate through trajectory and check constraints boolean exceededConstraintOutsideRegion = false; for (var point : trajectory.getStates()) { - var translation = point.poseMeters.getTranslation(); - - if (translation.getX() < Units.feetToMeters(10) - && translation.getY() < Units.feetToMeters(5)) { + if (ellipse.contains(point.poseMeters.getTranslation())) { assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05); } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) { exceededConstraintOutsideRegion = true; @@ -47,34 +40,4 @@ class EllipticalRegionConstraintTest { } assertTrue(exceededConstraintOutsideRegion); } - - @Test - void testIsPoseWithinRegion() { - double maxVelocity = Units.feetToMeters(3.0); - var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity); - - var regionConstraintNoRotation = - new EllipticalRegionConstraint( - new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)), - Units.feetToMeters(2.0), - Units.feetToMeters(4.0), - Rotation2d.kZero, - maxVelocityConstraint); - - assertFalse( - regionConstraintNoRotation.isPoseInRegion( - new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), Rotation2d.kZero))); - - var regionConstraintWithRotation = - new EllipticalRegionConstraint( - new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)), - Units.feetToMeters(2.0), - Units.feetToMeters(4.0), - Rotation2d.kCCW_Pi_2, - maxVelocityConstraint); - - assertTrue( - regionConstraintWithRotation.isPoseInRegion( - new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), Rotation2d.kZero))); - } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java index 3c8da41bcc..b244beacf9 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java @@ -4,11 +4,9 @@ package edu.wpi.first.math.trajectory; -import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; import edu.wpi.first.math.trajectory.constraint.RectangularRegionConstraint; @@ -19,22 +17,21 @@ import org.junit.jupiter.api.Test; class RectangularRegionConstraintTest { @Test void testConstraint() { - // Create constraints - double maxVelocity = Units.feetToMeters(3.0); - var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity); - var regionConstraint = - new RectangularRegionConstraint( + double maxVelocity = Units.feetToMeters(2.0); + var rectangle = + new Rectangle2d( new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)), - new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)), - maxVelocityConstraint); + new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(27.0))); - // Get trajectory - var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint)); + var trajectory = + TrajectoryGeneratorTest.getTrajectory( + List.of( + new RectangularRegionConstraint( + rectangle, new MaxVelocityConstraint(maxVelocity)))); - // Iterate through trajectory and check constraints boolean exceededConstraintOutsideRegion = false; for (var point : trajectory.getStates()) { - if (regionConstraint.isPoseInRegion(point.poseMeters)) { + if (rectangle.contains(point.poseMeters.getTranslation())) { assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05); } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) { exceededConstraintOutsideRegion = true; @@ -42,20 +39,4 @@ class RectangularRegionConstraintTest { } assertTrue(exceededConstraintOutsideRegion); } - - @Test - void testIsPoseWithinRegion() { - double maxVelocity = Units.feetToMeters(3.0); - var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity); - var regionConstraint = - new RectangularRegionConstraint( - new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)), - new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)), - maxVelocityConstraint); - - assertFalse(regionConstraint.isPoseInRegion(Pose2d.kZero)); - assertTrue( - regionConstraint.isPoseInRegion( - new Pose2d(Units.feetToMeters(3.0), Units.feetToMeters(14.5), Rotation2d.kZero))); - } } diff --git a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp index c39bb15aa0..72eacb7778 100644 --- a/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp @@ -2,11 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include - #include -#include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/trajectory/constraint/EllipticalRegionConstraint.h" #include "frc/trajectory/constraint/MaxVelocityConstraint.h" #include "trajectory/TestTrajectory.h" @@ -19,38 +16,20 @@ using namespace frc; TEST(EllipticalRegionConstraintTest, Constraint) { constexpr auto maxVelocity = 2_fps; + constexpr frc::Ellipse2d ellipse{{5_ft, 2.5_ft, 180_deg}, 5_ft, 2.5_ft}; auto config = TrajectoryConfig(13_fps, 13_fps_sq); - MaxVelocityConstraint maxVelConstraint(maxVelocity); - EllipticalRegionConstraint regionConstraint( - frc::Translation2d{5_ft, 2.5_ft}, 10_ft, 5_ft, 180_deg, maxVelConstraint); - config.AddConstraint(regionConstraint); - + config.AddConstraint( + EllipticalRegionConstraint{ellipse, MaxVelocityConstraint{maxVelocity}}); auto trajectory = TestTrajectory::GetTrajectory(config); bool exceededConstraintOutsideRegion = false; for (auto& point : trajectory.States()) { - auto translation = point.pose.Translation(); - if (translation.X() < 10_ft && translation.Y() < 5_ft) { + if (ellipse.Contains(point.pose.Translation())) { EXPECT_TRUE(units::math::abs(point.velocity) < maxVelocity + 0.05_mps); } else if (units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) { exceededConstraintOutsideRegion = true; } } - EXPECT_TRUE(exceededConstraintOutsideRegion); } - -TEST(EllipticalRegionConstraintTest, IsPoseInRegion) { - constexpr auto maxVelocity = 2_fps; - MaxVelocityConstraint maxVelConstraint(maxVelocity); - EllipticalRegionConstraint regionConstraintNoRotation( - frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, 0_deg, maxVelConstraint); - EXPECT_FALSE(regionConstraintNoRotation.IsPoseInRegion( - frc::Pose2d{2.1_ft, 1_ft, 0_rad})); - - EllipticalRegionConstraint regionConstraintWithRotation( - frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, 90_deg, maxVelConstraint); - EXPECT_TRUE(regionConstraintWithRotation.IsPoseInRegion( - frc::Pose2d{2.1_ft, 1_ft, 0_rad})); -} diff --git a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp index 8ec3a70a3f..68cb3885c6 100644 --- a/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp @@ -2,11 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include - #include -#include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/trajectory/constraint/MaxVelocityConstraint.h" #include "frc/trajectory/constraint/RectangularRegionConstraint.h" #include "trajectory/TestTrajectory.h" @@ -19,35 +16,20 @@ using namespace frc; TEST(RectangularRegionConstraintTest, Constraint) { constexpr auto maxVelocity = 2_fps; + constexpr frc::Rectangle2d rectangle{{1_ft, 1_ft}, {5_ft, 27_ft}}; auto config = TrajectoryConfig(13_fps, 13_fps_sq); - MaxVelocityConstraint maxVelConstraint(maxVelocity); - RectangularRegionConstraint regionConstraint(frc::Translation2d{1_ft, 1_ft}, - frc::Translation2d{5_ft, 27_ft}, - maxVelConstraint); - config.AddConstraint(regionConstraint); - + config.AddConstraint(RectangularRegionConstraint{ + rectangle, MaxVelocityConstraint{maxVelocity}}); auto trajectory = TestTrajectory::GetTrajectory(config); bool exceededConstraintOutsideRegion = false; for (auto& point : trajectory.States()) { - if (regionConstraint.IsPoseInRegion(point.pose)) { + if (rectangle.Contains(point.pose.Translation())) { EXPECT_TRUE(units::math::abs(point.velocity) < maxVelocity + 0.05_mps); } else if (units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) { exceededConstraintOutsideRegion = true; } } - EXPECT_TRUE(exceededConstraintOutsideRegion); } - -TEST(RectangularRegionConstraintTest, IsPoseInRegion) { - constexpr auto maxVelocity = 2_fps; - MaxVelocityConstraint maxVelConstraint(maxVelocity); - RectangularRegionConstraint regionConstraint(frc::Translation2d{1_ft, 1_ft}, - frc::Translation2d{5_ft, 27_ft}, - maxVelConstraint); - - EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d{})); - EXPECT_TRUE(regionConstraint.IsPoseInRegion(Pose2d{3_ft, 14.5_ft, 0_deg})); -}