Class CoordinateSystem

java.lang.Object
edu.wpi.first.math.geometry.CoordinateSystem

public class CoordinateSystem
extends Object
A helper class that converts Pose3d objects between different standard coordinate frames.
  • Constructor Details

  • Method Details

    • NWU

      public static CoordinateSystem NWU()
      Returns an instance of the North-West-Up (NWU) coordinate system.

      The +X axis is north, the +Y axis is west, and the +Z axis is up.

      Returns:
      An instance of the North-West-Up (NWU) coordinate system.
    • EDN

      public static CoordinateSystem EDN()
      Returns an instance of the East-Down-North (EDN) coordinate system.

      The +X axis is east, the +Y axis is down, and the +Z axis is north.

      Returns:
      An instance of the East-Down-North (EDN) coordinate system.
    • NED

      public static CoordinateSystem NED()
      Returns an instance of the North-East-Down (NED) coordinate system.

      The +X axis is north, the +Y axis is east, and the +Z axis is down.

      Returns:
      An instance of the North-East-Down (NED) coordinate system.
    • convert

      public static Translation3d convert​(Translation3d translation, CoordinateSystem from, CoordinateSystem to)
      Converts the given translation from one coordinate system to another.
      Parameters:
      translation - The translation to convert.
      from - The coordinate system the pose starts in.
      to - The coordinate system to which to convert.
      Returns:
      The given translation in the desired coordinate system.
    • convert

      public static Rotation3d convert​(Rotation3d rotation, CoordinateSystem from, CoordinateSystem to)
      Converts the given rotation from one coordinate system to another.
      Parameters:
      rotation - The rotation to convert.
      from - The coordinate system the rotation starts in.
      to - The coordinate system to which to convert.
      Returns:
      The given rotation in the desired coordinate system.
    • convert

      public static Pose3d convert​(Pose3d pose, CoordinateSystem from, CoordinateSystem to)
      Converts the given pose from one coordinate system to another.
      Parameters:
      pose - The pose to convert.
      from - The coordinate system the pose starts in.
      to - The coordinate system to which to convert.
      Returns:
      The given pose in the desired coordinate system.
    • convert

      public static Transform3d convert​(Transform3d transform, CoordinateSystem from, CoordinateSystem to)
      Converts the given transform from one coordinate system to another.
      Parameters:
      transform - The transform to convert.
      from - The coordinate system the transform starts in.
      to - The coordinate system to which to convert.
      Returns:
      The given transform in the desired coordinate system.