|
WPILibC++
2020.3.2
|
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... | |
| Pose2d & | operator+= (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 Translation2d & | Translation () const |
| Returns the underlying translation. More... | |
| const Rotation2d & | Rotation () 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... | |
Represents a 2d pose containing translational and rotational elements.
|
constexprdefault |
Constructs a pose at the origin facing toward the positive X axis.
(Translation2d{0, 0} and Rotation{0})
| frc::Pose2d::Pose2d | ( | Translation2d | translation, |
| Rotation2d | rotation | ||
| ) |
Constructs a pose with the specified translation and rotation.
| translation | The translational component of the pose. |
| rotation | The rotational component of the pose. |
| 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.
| 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. |
Obtain a new Pose2d from a (constant curvature) velocity.
See https://file.tavsys.net/control/state-space-guide.pdf section on nonlinear pose estimation for 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.
| 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 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.
| end | The end pose for the transformation. |
| bool frc::Pose2d::operator!= | ( | const Pose2d & | other | ) | const |
Checks inequality between this Pose2d and another object.
| other | The other object. |
| 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]
| other | The transform to transform the pose by. |
| 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.
| other | The transform to transform the pose by. |
| Transform2d frc::Pose2d::operator- | ( | const Pose2d & | other | ) | const |
Returns the Transform2d that maps the one pose to another.
| other | The initial pose of the transformation. |
| bool frc::Pose2d::operator== | ( | const Pose2d & | other | ) | const |
Checks equality between this Pose2d and another object.
| other | The other object. |
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.
| other | The pose that is the origin of the new coordinate frame that the current pose will be converted into. |
|
inline |
Returns the underlying rotation.
| 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.
| other | The transform to transform the pose by. |
|
inline |
Returns the underlying translation.