Package edu.wpi.first.math.geometry

Class Pose3d

java.lang.Object
edu.wpi.first.math.geometry.Pose3d
All Implemented Interfaces:
Interpolatable<Pose3d>
public class Pose3d
extends Object
implements Interpolatable<Pose3d>
Represents a 3D pose containing translational and rotational elements.

  • Constructor Summary

    Constructors 
    Constructor Description
    Pose3d()
    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.
    Pose3d​(Pose2d pose)
    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.

  • Method Summary

    Modifier and Type Method Description
    Pose3d div​(double scalar)
    Divides the current pose by a scalar.
    boolean equals​(Object obj)
    Checks equality between this Pose3d and another object.
    Pose3d exp​(Twist3d twist)
    Obtain a new Pose3d from a (constant curvature) velocity.
    Rotation3d getRotation()
    Returns the rotational component of the transformation.
    Translation3d getTranslation()
    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()  
    Pose3d interpolate​(Pose3d endValue, double t)
    Return the interpolated value.
    Twist3d log​(Pose3d end)
    Returns a Twist3d that maps this pose to the end pose.
    Transform3d minus​(Pose3d other)
    Returns the Transform3d that maps the one pose to another.
    Pose3d plus​(Transform3d other)
    Transforms the pose by the given transformation and returns the new transformed pose.
    Pose3d relativeTo​(Pose3d other)
    Returns the current pose relative to the given pose.
    Pose3d times​(double scalar)
    Multiplies the current pose by a scalar.
    Pose2d toPose2d()
    Returns a Pose2d representing this Pose3d projected into the X-Y plane.
    String toString()  
    Pose3d transformBy​(Transform3d other)
    Transforms the pose by the given transformation and returns the new pose.

    Methods inherited from class java.lang.Object

    clone, finalize, getClass, notify, notifyAll, wait, wait, wait

  • Constructor Details

    • Pose3d

      public Pose3d()
      Constructs a pose at the origin facing toward the positive X axis.

    • Pose3d

      public Pose3d​(Translation3d translation, Rotation3d rotation)
      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

      public Pose3d​(double x, double y, double z, Rotation3d rotation)
      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

      public Pose3d​(Pose2d pose)
      Constructs a 3D pose from a 2D pose in the X-Y plane.
      Parameters:
      pose - The 2D pose.

  • Method Details

    • plus

      public Pose3d plus​(Transform3d other)
      Transforms the pose by the given transformation and returns the new transformed pose.
      Parameters:
      other - The transform to transform the pose by.
      Returns:
      The transformed pose.

    • minus

      public Transform3d minus​(Pose3d other)
      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

      public Translation3d getTranslation()
      Returns the translation component of the transformation.
      Returns:
      The translational component of the pose.

    • getX

      public double getX()
      Returns the X component of the pose's translation.
      Returns:
      The x component of the pose's translation.

    • getY

      public double getY()
      Returns the Y component of the pose's translation.
      Returns:
      The y component of the pose's translation.

    • getZ

      public double getZ()
      Returns the Z component of the pose's translation.
      Returns:
      The z component of the pose's translation.

    • getRotation

      public Rotation3d getRotation()
      Returns the rotational component of the transformation.
      Returns:
      The rotational component of the pose.

    • times

      public Pose3d times​(double scalar)
      Multiplies the current pose by a scalar.
      Parameters:
      scalar - The scalar.
      Returns:
      The new scaled Pose3d.

    • div

      public Pose3d div​(double scalar)
      Divides the current pose by a scalar.
      Parameters:
      scalar - The scalar.
      Returns:
      The new scaled Pose3d.

    • transformBy

      public Pose3d transformBy​(Transform3d other)
      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.

    • relativeTo

      public Pose3d relativeTo​(Pose3d other)
      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

      public Pose3d exp​(Twist3d twist)
      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

      public Twist3d log​(Pose3d end)
      Returns a Twist3d 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.

    • toPose2d

      public Pose2d 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

      public String toString()
      Overrides:
      toString in class Object

    • equals

      public boolean equals​(Object obj)
      Checks equality between this Pose3d and another object.
      Overrides:
      equals in class Object
      Parameters:
      obj - The other object.
      Returns:
      Whether the two objects are equal or not.

    • hashCode

      public int hashCode()
      Overrides:
      hashCode in class Object

    • interpolate

      public Pose3d interpolate​(Pose3d endValue, double t)
      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 interface Interpolatable<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.