Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

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