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

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