Package edu.wpi.first.math.geometry
Class Pose3d
java.lang.Object
edu.wpi.first.math.geometry.Pose3d
- All Implemented Interfaces:
Interpolatable<Pose3d>
,ProtobufSerializable
,StructSerializable
,WPISerializable
public class Pose3d
extends Object
implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable
Represents a 3D pose containing translational and rotational elements.
-
Field Summary
Modifier and TypeFieldDescriptionstatic final Pose3d
A preallocated Pose3d representing the origin.static final Pose3dProto
Pose3d protobuf for serialization.static final Pose3dStruct
Pose3d struct for serialization. -
Constructor Summary
ConstructorDescriptionPose3d()
Constructs a pose at the origin facing toward the positive X axis.Pose3d
(double x, double y, double z, Rotation3d rotation) Constructs a pose with x, y, and z translations instead of a separate Translation3d.Constructs a 3D pose from a 2D pose in the X-Y plane.Pose3d
(Translation3d translation, Rotation3d rotation) Constructs a pose with the specified translation and rotation.Constructs a pose with the specified affine transformation matrix.Pose3d
(Distance x, Distance y, Distance z, Rotation3d rotation) Constructs a pose with x, y, and z translations instead of a separate Translation3d. -
Method Summary
Modifier and TypeMethodDescriptiondiv
(double scalar) Divides the current pose by a scalar.boolean
Checks equality between this Pose3d and another object.Obtain a new Pose3d from a (constant curvature) velocity.Returns the X component of the pose's translation in a measure.Returns the Y component of the pose's translation in a measure.Returns the Z component of the pose's translation in a measure.Returns the rotational component of the transformation.Returns the translation component of the transformation.double
getX()
Returns the X component of the pose's translation.double
getY()
Returns the Y component of the pose's translation.double
getZ()
Returns the Z component of the pose's translation.int
hashCode()
interpolate
(Pose3d endValue, double t) Return the interpolated value.Returns a Twist3d that maps this pose to the end pose.Returns the Transform3d that maps the one pose to another.plus
(Transform3d other) Transforms the pose by the given transformation and returns the new transformed pose.relativeTo
(Pose3d other) Returns the current pose relative to the given pose.rotateBy
(Rotation3d other) Rotates the pose around the origin and returns the new pose.times
(double scalar) Multiplies the current pose by a scalar.toMatrix()
Returns an affine transformation matrix representation of this pose.toPose2d()
Returns a Pose2d representing this Pose3d projected into the X-Y plane.toString()
transformBy
(Transform3d other) Transforms the pose by the given transformation and returns the new transformed pose.
-
Field Details
-
kZero
A preallocated Pose3d representing the origin.This exists to avoid allocations for common poses.
-
proto
Pose3d protobuf for serialization. -
struct
Pose3d struct for serialization.
-
-
Constructor Details
-
Pose3d
public Pose3d()Constructs a pose at the origin facing toward the positive X axis. -
Pose3d
Constructs a pose with the specified translation and rotation.- Parameters:
translation
- The translational component of the pose.rotation
- The rotational component of the pose.
-
Pose3d
Constructs a pose with x, y, and z translations instead of a separate Translation3d.- Parameters:
x
- The x component of the translational component of the pose.y
- The y component of the translational component of the pose.z
- The z component of the translational component of the pose.rotation
- The rotational component of the pose.
-
Pose3d
Constructs a pose with x, y, and z translations instead of a separate Translation3d. The X, Y, and Z translations will be converted to and tracked as meters.- Parameters:
x
- The x component of the translational component of the pose.y
- The y component of the translational component of the pose.z
- The z component of the translational component of the pose.rotation
- The rotational component of the pose.
-
Pose3d
Constructs a pose with the specified affine transformation matrix.- Parameters:
matrix
- The affine transformation matrix.- Throws:
IllegalArgumentException
- if the affine transformation matrix is invalid.
-
Pose3d
Constructs a 3D pose from a 2D pose in the X-Y plane.- Parameters:
pose
- The 2D pose.- See Also:
-
-
Method Details
-
plus
Transforms the pose by the given transformation and returns the new transformed pose. The transform is applied relative to the pose's frame. Note that this differs fromrotateBy(Rotation3d)
, which is applied relative to the global frame and around the origin.- Parameters:
other
- The transform to transform the pose by.- Returns:
- The transformed pose.
-
minus
Returns the Transform3d that maps the one pose to another.- Parameters:
other
- The initial pose of the transformation.- Returns:
- The transform that maps the other pose to the current pose.
-
getTranslation
Returns the translation component of the transformation.- Returns:
- The translational component of the pose.
-
getX
Returns the X component of the pose's translation.- Returns:
- The x component of the pose's translation.
-
getY
Returns the Y component of the pose's translation.- Returns:
- The y component of the pose's translation.
-
getZ
Returns the Z component of the pose's translation.- Returns:
- The z component of the pose's translation.
-
getMeasureX
Returns the X component of the pose's translation in a measure.- Returns:
- The x component of the pose's translation in a measure.
-
getMeasureY
Returns the Y component of the pose's translation in a measure.- Returns:
- The y component of the pose's translation in a measure.
-
getMeasureZ
Returns the Z component of the pose's translation in a measure.- Returns:
- The z component of the pose's translation in a measure.
-
getRotation
Returns the rotational component of the transformation.- Returns:
- The rotational component of the pose.
-
times
Multiplies the current pose by a scalar.- Parameters:
scalar
- The scalar.- Returns:
- The new scaled Pose3d.
-
div
Divides the current pose by a scalar.- Parameters:
scalar
- The scalar.- Returns:
- The new scaled Pose3d.
-
rotateBy
Rotates the pose around the origin and returns the new pose.- Parameters:
other
- The rotation to transform the pose by, which is applied extrinsically (from the global frame).- Returns:
- The rotated pose.
-
transformBy
Transforms the pose by the given transformation and returns the new transformed pose. The transform is applied relative to the pose's frame. Note that this differs fromrotateBy(Rotation3d)
, which is applied relative to the global frame and around the origin.- Parameters:
other
- The transform to transform the pose by.- Returns:
- The transformed pose.
-
relativeTo
Returns the current pose relative to the given 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.
-
exp
Obtain a new Pose3d from a (constant curvature) velocity.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 Twist3d(0.01, 0.0, 0.0, new new Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).- Returns:
- The new pose of the robot.
-
log
Returns a Twist3d that maps this pose to the end pose. If c is the output ofa.Log(b)
, thena.Exp(c)
would yield b.- Parameters:
end
- The end pose for the transformation.- Returns:
- The twist that maps this to end.
-
toMatrix
Returns an affine transformation matrix representation of this pose.- Returns:
- An affine transformation matrix representation of this pose.
-
toPose2d
Returns a Pose2d representing this Pose3d projected into the X-Y plane.- Returns:
- A Pose2d representing this Pose3d projected into the X-Y plane.
-
toString
-
equals
Checks equality between this Pose3d and another object. -
hashCode
-
interpolate
Description copied from interface:Interpolatable
Return the interpolated value. This object is assumed to be the starting position, or lower bound.- Specified by:
interpolate
in interfaceInterpolatable<Pose3d>
- Parameters:
endValue
- The upper bound, or end.t
- How far between the lower and upper bound we are. This should be bounded in [0, 1].- Returns:
- The interpolated value.
-