NextControl/dev.nextftc.control.geometry/Transform2d

Transform2d

data class Transform2d(val translation: Vector2d<DistanceUnit>, val rotation: Rotation2d)(source)

Represents a finite transformation (relative pose) in 2D space.

A Transform2d represents the change in pose from one pose to another. Unlike Twist2d which represents infinitesimal displacements in the Lie algebra, Transform2d represents finite relative transformations in SE(2).

Transform2d vs Twist2d vs Pose2d

Pose2d: An absolute pose in the global frame (position + heading)
Transform2d: A relative pose change - the transformation from one pose to another
Twist2d: An infinitesimal displacement in the Lie algebra (used for velocities)

Mathematical Representation

A transform can be thought of as a pose relative to another pose: \(T_{A \to B} = P_A^{-1} \circ P_B\)

Where \(P_A\) and \(P_B\) are absolute poses.

Common Use Cases

Relative Localization: Expressing one pose relative to another:

val robotPose = Pose2d(Vector2d(10.0.inches, 5.0.inches), Math.PI / 4)
val targetPose = Pose2d(Vector2d(15.0.inches, 8.0.inches), Math.PI / 2)
val transform = robotPose.relativeTo(targetPose)  // Transform from robot to target

Odometry Updates: Representing the change in pose between measurements:

val prevPose = odometry.getPose()
// ... robot moves ...
val currPose = odometry.getPose()
val delta = Transform2d(prevPose, currPose)  // How much the robot moved

Coordinate Frame Transformations: Converting between coordinate frames:

val cameraToRobot = Transform2d(
    Vector2d(6.0.inches, 0.0.inches),  // Camera is 6" forward
    Rotation2d.exp(0.0)
)
val targetInCameraFrame = Vector2d(24.0.inches, 12.0.inches)
val targetInRobotFrame = cameraToRobot * targetInCameraFrame

Example Usage

// Create a transform: 5 inches forward, rotated 30 degrees
val transform = Transform2d(
    Vector2d(5.0.inches, 0.0.inches),
    Math.PI / 6
)

// Apply transform to a pose
val initialPose = Pose2d(Vector2d(0.0.inches, 0.0.inches), 0.0)
val finalPose = initialPose + transform

// Compose transforms
val transform1 = Transform2d(Vector2d(5.0.inches, 0.0.inches), 0.0)
val transform2 = Transform2d(Vector2d(0.0.inches, 3.0.inches), Math.PI / 4)
val combined = transform1 + transform2

// Inverse transform
val inverse = transform.inverse()