NextControl/dev.nextftc.control.geometry/Pose2d

Pose2d

data class Pose2d(val position: Vector2d<DistanceUnit>, val heading: Rotation2d)(source)

Represents a 2D pose (position and orientation) in the Special Euclidean group SE(2).

A pose combines:

A position vector \(\mathbf{p} = (x, y)\) in 2D space
A heading (orientation) \(R \in SO(2)\)

SE(2) - The Special Euclidean Group

SE(2) is the group of rigid body transformations in 2D, consisting of rotations and translations. A pose can be represented as a transformation matrix:

\(g = \begin{bmatrix} R & \mathbf{p} \\ 0 & 1 \end{bmatrix} \in SE(2)\)

where \(R\) is a 2×2 rotation matrix and \(\mathbf{p}\) is a 2D translation vector.

Operations

Composition: Combining two poses gives a new pose: \(g_1 \circ g_2 = (R_1 R_2, R_1 \mathbf{p}_2 + \mathbf{p}_1)\)

Inverse: The inverse pose: \(g^{-1} = (R^T, -R^T \mathbf{p})\)

Exponential map: Converts a twist (velocity) to a pose: \(\exp: \mathfrak{se}(2) \to SE(2)\)

Logarithm map: Converts a pose to a twist: \(\log: SE(2) \to \mathfrak{se}(2)\)

Example Usage

// Create a pose at (10, 5) with 45-degree heading
val pose = Pose2d(Vector2d(10.0.inches, 5.0.inches), Math.PI / 4)

// Compose poses
val pose2 = Pose2d(Vector2d(1.0.inches, 0.0.inches), 0.0)
val composed = pose * pose2

// Transform a vector
val localPoint = Vector2d(1.0.inches, 0.0.inches)
val globalPoint = pose * localPoint

// Get relative pose
val relativePose = pose2 - pose

Constructors

constructor(position: Vector2d<DistanceUnit>, heading: Rotation2d)

constructor(position: Vector2d<DistanceUnit>, heading: Double)

Constructs a Pose2d from a position vector and a heading angle.

constructor(x: Distance, y: Distance, heading: Rotation2d)

Constructs a Pose2d from x and y distance measurements and a heading rotation.

constructor(x: Distance, y: Distance, heading: Angle)

Constructs a Pose2d from x and y distance measurements and a heading angle.

constructor(x: Double, y: Double, heading: Rotation2d)

Constructs a Pose2d from x and y coordinates (in inches) and a heading rotation.

constructor(x: Double, y: Double, heading: Double)

Constructs a Pose2d from x and y coordinates (in inches) and a heading angle (in radians).

Types

object Companion

Properties

val heading: Rotation2d

the orientation as a Rotation2d

val position: Vector2d<DistanceUnit>

the position vector in 2D space

Functions

closestOnSegment

fun closestOnSegment(start: Pose2d, end: Pose2d): Pose2d

Finds the closest pose on a line segment between two poses.

closestParameterOnSegment

fun closestParameterOnSegment(start: Pose2d, end: Pose2d): Double

Finds the parameter t where the distance between two poses is minimized.

fun distanceTo(other: Pose2d): Double

Computes the linear distance to another pose.

fun inverse(): Pose2d

Computes the inverse of this pose.

fun lerp(other: Pose2d, t: Double): Pose2d

Linear interpolation (lerp) toward another pose.

fun log(): Twist2d

Computes the twist (logarithm map) of this pose.

operator fun minus(t: Pose2d): Twist2d

Computes the twist (logarithm map) from another pose to this pose.

fun minusExp(t: Pose2d): Pose2d

Computes the relative pose from another pose to this pose.

operator fun plus(transform: Transform2d): Pose2d

Applies a transform to this pose.

operator fun plus(t: Twist2d): Pose2d

Adds a twist to this pose using the exponential map.

fun relativeTo(target: Pose2d): Transform2d

Computes the transform from this pose to another pose.

operator fun times(p: Pose2d): Pose2d

Composes this pose with another pose.

operator fun times(pv: PoseVelocity2d): PoseVelocity2d

Transforms a pose velocity by this pose's rotation.

operator fun times(v: Vector2d<DistanceUnit>): Vector2d<DistanceUnit>

Transforms a vector by this pose.