WPILibC++  2021.2.2
frc::Pose2d Class Reference

Represents a 2d pose containing translational and rotational elements. More...

#include <Pose2d.h>

Public Member Functions

constexpr Pose2d ()=default
 Constructs a pose at the origin facing toward the positive X axis. More...
 
 Pose2d (Translation2d translation, Rotation2d rotation)
 Constructs a pose with the specified translation and rotation. More...
 
 Pose2d (units::meter_t x, units::meter_t y, Rotation2d rotation)
 Convenience constructors that takes in x and y values directly instead of having to construct a Translation2d. More...
 
Pose2d operator+ (const Transform2d &other) const
 Transforms the pose by the given transformation and returns the new transformed pose. More...
 
Pose2doperator+= (const Transform2d &other)
 Transforms the current pose by the transformation. More...
 
Transform2d operator- (const Pose2d &other) const
 Returns the Transform2d that maps the one pose to another. More...
 
bool operator== (const Pose2d &other) const
 Checks equality between this Pose2d and another object. More...
 
bool operator!= (const Pose2d &other) const
 Checks inequality between this Pose2d and another object. More...
 
const Translation2dTranslation () const
 Returns the underlying translation. More...
 
units::meter_t X () const
 Returns the X component of the pose's translation. More...
 
units::meter_t Y () const
 Returns the Y component of the pose's translation. More...
 
const Rotation2dRotation () const
 Returns the underlying rotation. More...
 
Pose2d TransformBy (const Transform2d &other) const
 Transforms the pose by the given transformation and returns the new pose. More...
 
Pose2d RelativeTo (const Pose2d &other) const
 Returns the other pose relative to the current pose. More...
 
Pose2d Exp (const Twist2d &twist) const
 Obtain a new Pose2d from a (constant curvature) velocity. More...
 
Twist2d Log (const Pose2d &end) const
 Returns a Twist2d that maps this pose to the end pose. More...
 

Detailed Description

Represents a 2d pose containing translational and rotational elements.

Constructor & Destructor Documentation

◆ Pose2d() [1/3]

constexpr frc::Pose2d::Pose2d ( )
constexprdefault

Constructs a pose at the origin facing toward the positive X axis.

(Translation2d{0, 0} and Rotation{0})

◆ Pose2d() [2/3]

frc::Pose2d::Pose2d ( Translation2d  translation,
Rotation2d  rotation 
)

Constructs a pose with the specified translation and rotation.

Parameters
translationThe translational component of the pose.
rotationThe rotational component of the pose.

◆ Pose2d() [3/3]

frc::Pose2d::Pose2d ( units::meter_t  x,
units::meter_t  y,
Rotation2d  rotation 
)

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

Parameters
xThe x component of the translational component of the pose.
yThe y component of the translational component of the pose.
rotationThe rotational component of the pose.

Member Function Documentation

◆ Exp()

Pose2d frc::Pose2d::Exp ( const Twist2d twist) const

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
twistThe 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.

◆ Log()

Twist2d frc::Pose2d::Log ( const Pose2d end) const

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
endThe end pose for the transformation.
Returns
The twist that maps this to end.

◆ operator!=()

bool frc::Pose2d::operator!= ( const Pose2d other) const

Checks inequality between this Pose2d and another object.

Parameters
otherThe other object.
Returns
Whether the two objects are not equal.

◆ operator+()

Pose2d frc::Pose2d::operator+ ( const Transform2d other) const

Transforms the pose by the given transformation and returns the new transformed pose.

[x_new] [cos, -sin, 0][transform.x] [y_new] += [sin, cos, 0][transform.y] [t_new] [0, 0, 1][transform.t]

Parameters
otherThe transform to transform the pose by.
Returns
The transformed pose.

◆ operator+=()

Pose2d& frc::Pose2d::operator+= ( const Transform2d other)

Transforms the current pose by the transformation.

This is similar to the + operator, except that it mutates the current object.

Parameters
otherThe transform to transform the pose by.
Returns
Reference to the new mutated object.

◆ operator-()

Transform2d frc::Pose2d::operator- ( const Pose2d other) const

Returns the Transform2d that maps the one pose to another.

Parameters
otherThe initial pose of the transformation.
Returns
The transform that maps the other pose to the current pose.

◆ operator==()

bool frc::Pose2d::operator== ( const Pose2d other) const

Checks equality between this Pose2d and another object.

Parameters
otherThe other object.
Returns
Whether the two objects are equal.

◆ RelativeTo()

Pose2d frc::Pose2d::RelativeTo ( const Pose2d other) const

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
otherThe 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()

const Rotation2d& frc::Pose2d::Rotation ( ) const
inline

Returns the underlying rotation.

Returns
Reference to the rotational component of the pose.

◆ TransformBy()

Pose2d frc::Pose2d::TransformBy ( const Transform2d other) const

Transforms the pose by the given transformation and returns the new pose.

See + operator for the matrix multiplication performed.

Parameters
otherThe transform to transform the pose by.
Returns
The transformed pose.

◆ Translation()

const Translation2d& frc::Pose2d::Translation ( ) const
inline

Returns the underlying translation.

Returns
Reference to the translational component of the pose.

◆ X()

units::meter_t frc::Pose2d::X ( ) const
inline

Returns the X component of the pose's translation.

Returns
The x component of the pose's translation.

◆ Y()

units::meter_t frc::Pose2d::Y ( ) const
inline

Returns the Y component of the pose's translation.

Returns
The y component of the pose's translation.

The documentation for this class was generated from the following file: