Understanding The New Geometry Classes

Hi everyone!

WPILib are adding geometry classes for the 2020 update, inspired by Team 254’s code.
While I do understand some of them, I find others a bit harder to understand.
My problem is not with understanding syntax and math itself, but rather what each class represents.
I have read through the descriptions of the classes but I still don’t fully understand.
Specifically, What is Twist2D meant for?

In general, I would love it if someone could try and explain these classes, and maybe provide a theoretical use case for them. I’m sure that it would be helpful for others as well!

Thanks a lot!

1 Like

Well, if you want the explanation from abstract algebra…

SO(2) is the special orthogonal group in dimension 2. They represent a 2D rotation (aka Rotation2d). SE(2) is the special euclidean group in dimension 2. They represent a 2D rotation (Rotation2d) and a 2D translation (Translation2d), which we call a Pose2d. In other words, Pose2d is an element of SE(2).

A twist is an element of the tangent space of SE(2) (like the tangential distance traveled by the robot along an arc in SE(2)). We use the “pose exponential” (see Pose2d::Exp()) to map a twist (an element of the tangent space) to an element of SE(2). In other words, we map a twist to a pose.

We call it a pose exponential because it’s an exponential map onto a pose. The term exponential is used because the solution for integrating a change in something (like a slope or tangent) over time is usually an exponential. For example, dx/dt = ax has the solution x = x_0 e^(at).

We use the pose exponential to take encoder measurement deltas and gyro angle deltas (which are in the tangent space and are thus a twist) and turn them into a change in pose. This gets added to the pose from the last update. There’s a PR for odometry classes that does this exact thing for you, but it hasn’t been merged yet.

I believe that there’s an explanation about this exact class from 254 here: https://www.chiefdelphi.com/t/team-254-presents-2019-code-for-backlash/361571/18?u=jonathano

I’ll give a slightly less advanced explanation that doesn’t use Lie theory.

Twist2d is usually used to hold a “difference” between two poses. The most common application of Twist2d is “nonlinear pose estimation”, which uses information about change in heading and position, and the assumption that between updates we move along an arc, not a straight line, to estimate a more accurate change in overall robot pose.

Section 11.1 of this excellent book describes the motivations behind nonlinear pose estimation, and how to derive the exp function used below.

This file shows an example of using the new differential drive kinematics class and Twist2d to estimate the robot’s pose.

Finally, here is a more compact psuedocode example of how you would use Twist2d for nonlinear pose estimation:

// Note that the below deltas are not time-normalized; i.e. the units are distance/time since last update
double deltaLeft = currentLeftPosition - prevLeftPosition;
double deltaRight = currentRightPosition - prevRightPosition;
double deltaRotation currentRotation - prevRotation;

// Here we perform forward kinematics, which converts wheel deltas to a whole-robot delta
double dx = (deltaLeft + deltaRight) / 2;
// Note that dy is set to 0 here. We make the assumption that we're just driving straight and turning.
Twist2d delta = new Twist2d(dx, 0.0, deltaRotation);
// This is where we use the pose exponential, which converts our whole-robot delta to a Pose2d, and then integrate that new Pose2d with the previous pose
Pose2d newPose = previousPose.exp(delta);
1 Like

Except that those are not acceptable use cases for this library. The WPILib Twist2d should only represent a “difference” between two poses as Tyler and pietroglyph described above.

A separate ChassisSpeeds object will be added to represent speed.

My bad, I’m not super familiar with the new 2020 update as I’m not doing software on the team anymore… You’re probably correct :slight_smile:

It’s not exactly the difference between two poses. It’s an arc length in x and y in the robot’s coordinate frame. The gyro angle is a delta though.

Yeah, difference wasn’t quite the right word, added a quotation to clarify.