Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01: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();
}
}