PoseVelocity2d

Represents the velocity of a 2D pose in the global (field) frame.

A pose velocity describes how a pose changes over time and consists of:

• Linear velocity: \(\mathbf{v}\) - the velocity of the position in field coordinates

• Angular velocity: \(\omega\) - the rate of change of heading (frame-invariant)

PoseVelocity2d vs ChassisVelocities

PoseVelocity2d represents velocities in the global (field) frame:

• The x-axis and y-axis are fixed to the field

• Linear velocities are expressed in field coordinates

• Natural for trajectory planning and global navigation

• Used in Lie algebra operations and pose derivatives

ChassisVelocities represents velocities in the robot's local frame (body frame):

• The x-axis points forward, y-axis points left (from robot's perspective)

• Linear velocities are relative to the robot's orientation

• Natural for robot control and motor commands

• Used in odometry and state estimation

Relationship to Twists

A pose velocity is the time derivative of a pose and lives in the tangent space of SE(2), also known as the Lie algebra \(\mathfrak{se}(2)\).

For a pose \(g(t)\), the pose velocity is \(\dot{g}(t) = (\mathbf{v}, \omega)\).

Frame Conversion

To convert from chassis (local) to pose (global) velocities:

val poseVel = PoseVelocity2d(
    linearVel = currentPose.heading * chassisVel.linearVel,
    angVel = chassisVel.angVel  // Angular velocity is frame-invariant
)

To convert from pose (global) to chassis (local) velocities:

val chassisVel = ChassisVelocities(
    linearVel = currentPose.heading.inverse() * poseVel.linearVel,
    angVel = poseVel.angVel
)

Example Usage

// Robot moving at 10 in/s in the field's x-direction, 5 in/s in y-direction
val poseVel = PoseVelocity2d(
    linearVel = Vector2d(10.0.inchesPerSecond, 5.0.inchesPerSecond),
    angVel = 0.5.radiansPerSecond
)

// Transform to robot's local frame
val chassisVel = ChassisVelocities(
    linearVel = pose.heading.inverse() * poseVel.linearVel,
    angVel = poseVel.angVel
)