Pose zones

[Technologyman00]

Is your feature request related to a problem? Please describe. I have noticed that as AprilTags and pose estimation take over there isn't a built in way to know if your robot is in a given zone. Which can help significantly for automatic scoring. This could be used for Auto, but I think it is better for Teleop.

Describe the solution you'd like New classes and objects like RectangularPose2DZone and CircularPose2DZone

RetangularPose2DZone would take 3 Coordinates for the constructor

• 2 for the top left and bottom right of the rectangle
• 1 for a point for the Pose to face in that Zone

CircularPose2DZone would take 2 Coordinates and a radius for the constructor

• 1 Coordinate and the radius for the zone of the circle
• The other coordinate for the direction to face in that Zone

Each would have methods that are:

• isPoseinZone(Pose2D Pose) - Returns a boolean for if the Inputted Pose is in the PoseZone
• closestPoseinZone(Pose2D Pose) - Returns a Pose2D of the closest Pose to the inputted Pose in the Zone
• MirrorZone - Returns a PoseZone that is mirrored across the field
• FlipZone - Returns a PoseZone that is flipped the short way across the field

Describe alternatives you've considered Teams could write their own logic for this, but this implementation feels a lot cleaner.

Additional context There could be reasons that teams would prefer not to use something like this instead of just using the apriltags directly, but I believe using this implementation allows the PoseEstimator to do the heavy lifting of where the robot is and where it should be facing and if the Apriltags are being blocked or cannot be detected the robot still has an idea of where it should be going for scoring.

[calcmogul]

In my opinion, this would make more sense as geometric primitives like Rectangle2d and Ellipse2d (in the geometry package) with a bool contains(Translation2d) method. We could copy the logic from our ellipse and rectangle trajectory constraints and make the constraints use these new classes instead.

https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java

We could also have transformation methods like transformBy() and rotateBy() to make it easier to place them at the right place relative to known locations.

1 for a point for the Pose to face in that Zone

I don't understand what this is used for.

closestPoseinZone(Pose2D Pose) - Returns a Pose2D of the closest Pose to the inputted Pose in the Zone

When and why would someone use this function instead of Pose2d.nearest()?

MirrorZone - Returns a PoseZone that is mirrored across the field FlipZone - Returns a PoseZone that is flipped the short way across the field

What's the difference between mirroring and flipping? They sound like the same operation. Are you essentially suggesting flipX() and flipY()?

[Technologyman00]

1 for a point for the Pose to face in that Zone

I don't understand what this is used for.

Excuse the crude drawing but it is to have the robot face the goal in that zone. I think using something like this would make a nice way to both provide limits of where a robot can score an object from and provide direction to where the goal is relative to the robot.

closestPoseinZone(Pose2D Pose) - Returns a Pose2D of the closest Pose to the inputted Pose in the Zone

When and why would someone use this function instead of Pose2d.nearest()?

They certainly do about the same thing, but the zone wouldn't have to contain a list of every pose allowed, just the acceptable zone.

MirrorZone - Returns a PoseZone that is mirrored across the field FlipZone - Returns a PoseZone that is flipped the short way across the field

What's the difference between mirroring and flipping? They sound like the same operation. Are you essentially suggesting flipX() and flipY()?

Yes, sorry that is exactly what I meant.

[calcmogul]

it is to have the robot face the goal in that zone. I think using something like this would make a nice way to both provide limits of where a robot can score an object from and provide direction to where the goal is relative to the robot.

This doesn't sound like something that needs to be in WPILib because WPILib tries to be a composable library rather than a framework. It's easy enough for the user to write it themselves, and it expresses their intent well.

var estimator = new SwerveDrivePoseEstimator(...);

final var scoringZone1 = Rectangle2d(...);
final var target1 = new Pose2d(...);

final var scoringZone2 = Rectangle2d(...);
final var target2 = new Pose2d(...);

public void loop() {
  final var robotPose = estimator.getEstimatedPosition();
  if (scoringZone1.contains(robotPose.getTranslation())) {
    // Aim at target 1
  } else if (scoringZone2.contains(robotPose.getTranslation())) {
    // Aim at target 2
  }
}

[Technologyman00]

Ok I think that is a fair point. I wasn't aware Rectangle2D could compare to Pose2D. But I still think that Pose2D should have a method to calculate how much to rotate to look at a point.

Something like

Pose2D.rotationToFace(translation2D) returns Rotation2D

[calcmogul]

I wasn't aware Rectangle2D could compare to Pose2D.

It can't, because Rectangle2d doesn't exist yet. I'm saying the hypothetical API could take a Translation2d, and Pose2d has a translation accessor already.

But I still think that Pose2D should have a method to calculate how much to rotate to look at a point.

You can already do that like this:

Pose2d robotPose;
Pose2d target;
double heading = target.getTranslation().minus(robotPose.getTranslation()).getAngle();

It's essentially doing the following:

double heading = Math.atan2(target.getY() - robotPose.getY(), target.getX() - robotPose.getX());

[Technologyman00]

Ok I think that sounds good.

I think the next challenge is defining the rectangle. As it could be desired to have it at an angle vs parallel to the field.

[calcmogul]

You can test intersection with CCW-rotated shapes by rotating the test point CW around the shape's origin before performing the check. It's essentially rotating the test point into the shape's coordinate frame.

Both Rectangle2d and Ellipse2d can use the following constructor arguments:

• Translation2d center
• double xWidth (in unrotated coordinate frame)
• double yWidth (in unrotated coordinate frame)
• Rotation2d rotation

A second Ellipse2d constructor could take the following for a circle:

• Translation2d center
• double radius

[Technologyman00]

Welp I am out of my league. This is as far as I could get https://github.com/Technologyman00/allwpilib/commit/407ab21f92a7a9ab2935b2fb61b172e9b1f3fc3f

It doesn't build as I am unfamiliar with the Proto/Struct/Test stuff

[Gold856]

You need to regenerate the protobuf Java file by running wpimath/generate_quickbuf.py. You'll need protoc, and the quickbuf plugin. Pass the paths to protoc and the quickbuf plugin as arguments to generate_quickbuf.py, and commit the files. That will give you the protobuf dependencies you need.

[Gold856]

Also duplicate of #4090.

[Technologyman00]

@Gold856 is that something I need to worry about for contributing?

[Gold856]

Not sure what you're referencing. You definitely need to regenerate the protobuf files to get the protobuf dependencies. If you are referencing #4090, it's up to you whether or not to implement everything in that issue in addition to this issue.

[KangarooKoala]

Looks like this would be your first time contributing, so if you plan to contribute this yourself it would be good to read over the code of conduct and information for contributors, but if you have more questions (or comments on things that could be improved about those), then you could ask here or in the programming channels in the FRC Discord.