Pose2d

class wpimath.geometry.Pose2d(*args, **kwargs)

Bases: pybind11_builtins.pybind11_object

Represents a 2d pose containing translational and rotational elements.

Overloaded function.

  1. __init__(self: wpimath.geometry._geometry.Pose2d) -> None

Constructs a pose at the origin facing toward the positive X axis. (Translation2d{0, 0} and Rotation{0})

  1. __init__(self: wpimath.geometry._geometry.Pose2d, translation: wpimath.geometry._geometry.Translation2d, rotation: wpimath.geometry._geometry.Rotation2d) -> None

Constructs a pose with the specified translation and rotation.

Parameters
  • translation – The translational component of the pose.

  • rotation – The rotational component of the pose.

  1. __init__(self: wpimath.geometry._geometry.Pose2d, x: meters, y: meters, rotation: wpimath.geometry._geometry.Rotation2d) -> None

Convenience constructors that takes in x and y values directly instead of having to construct a Translation2d.

Parameters
  • x – The x component of the translational component of the pose.

  • y – The y component of the translational component of the pose.

  • rotation – The rotational component of the pose.

  1. __init__(self: wpimath.geometry._geometry.Pose2d, x: meters, y: meters, angle: radians) -> None

X()meters

Returns the X component of the pose’s translation.

Returns

The x component of the pose’s translation.

Y()meters

Returns the Y component of the pose’s translation.

Returns

The y component of the pose’s translation.

exp(twist: wpimath.geometry.Twist2d)wpimath.geometry.Pose2d

Obtain a new Pose2d from a (constant curvature) velocity.

See https://file.tavsys.net/control/controls-engineering-in-frc.pdf section 10.2 “Pose exponential” for a derivation.

The twist is a change in pose in the robot’s coordinate frame since the previous pose update. When the user runs exp() on the previous known field-relative pose with the argument being the twist, the user will receive the new field-relative pose.

“Exp” represents the pose exponential, which is solving a differential equation moving the pose forward in time.

Parameters

twist – The change in pose in the robot’s coordinate frame since the previous pose update. For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5 degrees since the previous pose update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}

Returns

The new pose of the robot.

static fromFeet(*args, **kwargs)

Overloaded function.

  1. fromFeet(x: feet, y: feet, r: wpimath.geometry._geometry.Rotation2d) -> wpimath.geometry._geometry.Pose2d

  2. fromFeet(x: feet, y: feet, angle: radians) -> wpimath.geometry._geometry.Pose2d

log(end: wpimath.geometry.Pose2d)wpimath.geometry.Twist2d

Returns a Twist2d that maps this pose to the end pose. If c is the output of a.Log(b), then a.Exp(c) would yield b.

Parameters

end – The end pose for the transformation.

Returns

The twist that maps this to end.

relativeTo(other: wpimath.geometry.Pose2d)wpimath.geometry.Pose2d

Returns the other pose relative to the current pose.

This function can often be used for trajectory tracking or pose stabilization algorithms to get the error between the reference and the current pose.

Parameters

other – The pose that is the origin of the new coordinate frame that the current pose will be converted into.

Returns

The current pose relative to the new origin pose.

rotation()wpimath.geometry.Rotation2d

Returns the underlying rotation.

Returns

Reference to the rotational component of the pose.

transformBy(other: wpimath.geometry.Transform2d)wpimath.geometry.Pose2d

Transforms the pose by the given transformation and returns the new pose. See + operator for the matrix multiplication performed.

Parameters

other – The transform to transform the pose by.

Returns

The transformed pose.

translation()wpimath.geometry.Translation2d

Returns the underlying translation.

Returns

Reference to the translational component of the pose.