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);