17. Where Is the Robot? Poses and Odometry#

Why it matters#

So far the robot has only known things about itself: how fast a wheel spins, how far it has rolled, which way it's facing. To score in autonomous, line up on a goal, or follow a path, it needs to know where it is on the field.

That's called the robot's pose, and keeping track of it is called pose estimation. This lesson covers how WPILib describes a pose and how odometry tracks it from the encoders and gyro you already have. Lesson 18 adds cameras to correct it.

Read this#

• Coordinate System (WPILib)
• Kinematics and Odometry (WPILib)
• Differential Drive Odometry (WPILib)

Walkthrough#

What a pose is#

A pose is three numbers:

WPILib stores them in a Pose2d, which is a Translation2d (X and Y) plus a Rotation2d (heading):

Pose2d pose = new Pose2d(2.0, 3.0, Rotation2d.fromDegrees(45));

pose.getX();                         // 2.0 meters
pose.getY();                         // 3.0 meters
pose.getRotation().getDegrees();     // 45.0
pose.getTranslation();               // just the (2.0, 3.0) part

A few things you can do with poses:

// How far apart are two poses? (straight-line distance in meters)
double meters = pose.getTranslation().getDistance(target.getTranslation());

// How far do I still have to turn to face the target's heading?
Rotation2d turnLeft = target.getRotation().minus(pose.getRotation());

// Where would I be after driving 1 m forward from here? (forward = the robot's own front)
Pose2d ahead = pose.transformBy(new Transform2d(1.0, 0.0, Rotation2d.kZero));

The field's coordinates#

WPILib uses one coordinate system for the whole field, called the field frame, and it's the same for both alliances:

• (0, 0) is the corner on the blue alliance wall, on the right as the blue drivers look at the field.
• +X points away from the blue wall, toward the red wall.
• +Y points to the left, as seen from the blue driver station.
• Heading 0° faces +X (toward red). Turning counterclockwise (left, seen from above) makes it go up.

That last rule is the same one the gyro followed in Lab 8. WPILib uses "counterclockwise is positive" everywhere.

There's also the robot frame: +X straight out the robot's front, +Y out its left side. Camera positions ("15 cm forward of center, 30 cm up") are written in the robot frame.

Three ways to know the pose#

Competition robots use fusion. It starts with good odometry, so that's first.

How odometry works#

Every 20 ms, odometry asks the sensors two questions:

1. How far did the robot roll since last loop? The average of the left and right encoder changes.
2. Which way was it facing? The gyro heading.

A step of 1 cm while facing 0° adds 1 cm to X. The same step facing 90° adds 1 cm to Y. Add up 50 of those steps a second and you have a running position. This is dead reckoning, the same way sailors navigated before GPS.

WPILib does the math in DifferentialDriveOdometry:

// Once, in the constructor: remember the starting readings
m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d(), getLeftMeters(), getRightMeters());

// Every loop, in periodic(): add this loop's step
m_odometry.update(m_gyro.getRotation2d(), getLeftMeters(), getRightMeters());

// Any time: where are we?
Pose2d pose = m_odometry.getPoseMeters();

Lab 11 adds exactly this to the West Coast Drive.

Resetting the pose#

Odometry starts wherever you tell it to, (0, 0, 0°) by default. To move it:

m_odometry.resetPosition(m_gyro.getRotation2d(), getLeftMeters(), getRightMeters(), newPose);

You always pass the sensors' current readings, because odometry measures changes from them. You don't reset the encoders or gyro themselves. If a sensor reading suddenly jumps (because you reset it), odometry thinks the robot moved.

At the start of autonomous, teams reset the pose to where the robot really starts on the field, such as "touching the blue wall, 4.0 m from the right side, facing red." PathPlanner autos do this for you.

Why odometry drifts#

Every error gets added to the pose and stays there:

A well-calibrated drivetrain is usually within a few centimeters after a short auto, and off by much more after a full match of pushing. That's why vision matters.

Swerve odometry#

Swerve robots use the same idea with SwerveDriveOdometry. Instead of two encoder distances, each of the four modules reports how far it rolled and which way it was pointing. Lesson 21 covers swerve.

Seeing the pose#

Two ways to look at the pose:

• Elastic: a Field2d (sent with SmartDashboard.putData) shows the robot on a field picture. Good for drivers and quick checks.
• AdvantageScope: log the Pose2d and drag it onto the 2D or 3D field. You can replay a whole match afterward. See Lesson 20.

Try it#

• Do Lab 11
• On paper: the robot is at (2, 1) facing 90°. It drives 1 m forward. What's its new pose?
• On paper: the robot faces 30° and the target faces 120°. How far, and which way, must it turn?

Check yourself#

Parts of this lesson are adapted from the Introduction to Pose Estimation and Odometry lessons of Mantik by Itkan Robotics (CC BY 4.0).