Swerve pose estimator origin

Is the pose estimator origin always based on the blue alliance wall right side? Or does it change to red alliance wall right side if you’re on red? If it does change, is there a way to configure it to be blue alliance wall right side at all times?

You do have to change the origin. Here’s an example:

My question is not about the PhotonPoseEstimator, but the SwerveDrivePoseEstimator.

My bad–for drivetrain pose estimators you do not have to transform the origin, everything is based on the blue alliance side. Auton paths and AprilTag locations need to be flipped but all your poses will be based on the blue side.

I think this is really your choice, but it does affect your code.

For 2023, the field was not rotationally symmetric, it was mirrored, so it was odd. My team (me) decided to stick with the origin always being at Blue, left corner (bottom left on most field drawings). So, we had to be pretty careful to do the correct flips everywhere.

As for PhotonPoseEstimator, it depends on what you did. It is defined by the Fieldmap that it is fed. If you stick with origin = blue, left, you don’t do anything. If you want to change the origin, then you need to call AprilTagFieldLayout.setOrigin().

BTW, note that this is a bit of a tricky situation, which is why, after a bunch of flip-flops, my team stuck with a fixed origin and flipped things “live”.

The problem is that the Robot does not know its Alliance color reliably until it connects to the FMS, so you need to code such that the decision can change (including in case of a reboot during a match). There was a long thread about this maybe in January?

Also, be careful if you do notice a Red-Blue change: at least some of the methods do NOT work the way you might expect. I think the field map is one: if you setOrigin(Blue), it does nothing. If you call setOrigin(Red), it flips the coordinates. If you then want to flip it back to Blue, you actually need to call “setOrigin(Red)”!!

This is a WPILib standard, by the way, so it’s not so much of a personal choice :confused:

But you are correct on the weirdness of this year’s game. Last year you could run auton paths with no modification for both alliances.

I hear and agree. However, other libraries would automatically switch in many methods (for instance PathPlanner).

We could not use their “switch to Red” method because it assumed that you had already switched the origin to Red-right. As far as I could find at the time (maybe solved now), there was no way to deal with it. We ended writing a small Python script to mirror the paths, and then loaded both Blue and Red trajectories, and selecting when auto started.

What do you mean? We used blue as the origin and the PathPlannerLib trajectory loader worked. You do have to flip the starting point on the X-axis though.

As best as I could tell (and I am pretty sure I tried it in the simulation), the PP switch code did not purely flip the path. It made the assumption that, for Red, you used Red-Right as your field origin, so when you asked for it to switch the trajectory to Red, it would actually flip it in Y, not in X.

If you want to leave the origin at Blue-Right, then you needed to only flip the trajectory in X, which was not implemented (when I checked).

You do have to flip the starting point on the X-axis though.

Yes, I think we are saying the same thing. But I could not find a method in PP to do that. The solution I found was to do it in the saved files, before loading them in to PP.

The pose estimators are not "based on the blue alliance side- they are based on the origin of whatever data you provide them.

Not sure what this is referring to- if it’s in reference to the AprilTagFieldLayout default coordinate system, you can redefine the origin- either using the right corner of each alliance wall, or with an arbitrary Pose3d.

I was under the impression that WPILib has used blue alliance origin for some time now–is this not the case anymore?

The past few years, wpilib has used the rightmost corner (when viewed from behind the wall) of each alliance wall as the origin for each alliance, with the positive X axis pointing towards the opposing alliance wall and the positive Y axis pointing towards the left side field border (documented here). This worked well because it meant that all coordinates were positive, and because the field has historically been rotationally symmetric, any given coordinate would match up relative to field elements regardless of alliance (ie, you can use one set of coordinates for both sides).

Note that this is a pretty loose standard- there’s not much (if anything?) in the wpilib libraries that requires you use those origin points. Pathweaver generates paths in that coordinate system, but paths are trivial to transform to the starting pose of your choice.

Obviously, that doesn’t quite work out when the field is not rotationally symmetric. The AprilTagFieldLayout class provides the setOrigin function that takes a Pose3d relative to the default blue alliance coordinate system so that the user can use whatever origin they choose.
Coordinates used for navigation (paths, coordinates of field elements, etc) will need to be transformed at some point to use them on both sides, in order to stay in a NWU coordinate system.

1 Like