Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make trajectory constraints use Rectangle2d and Ellipse2d (#6901)

Browse Source
This commit is contained in:
Tyler Veness
2024-08-01 16:46:23 -07:00
committed by GitHub
parent ddd64aa70c
commit 685c732568
8 changed files with 108 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
View File

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

37
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java
View File

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

57
wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
View File

@@ -7,12 +7,14 @@
#include <concepts>
#include <limits>
#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

47
wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
View File

@@ -7,11 +7,12 @@
#include <concepts>
#include <limits>
#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

57
wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java
View File

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

41
wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java
View File

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

29
wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
View File

@@ -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 <vector>
#include <gtest/gtest.h>
#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}));
}

26
wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
View File

@@ -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 <vector>
#include <gtest/gtest.h>
#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}));
}