[wpimath] Add RotateBy() function to pose classes (#5491)

Fixes #5472.

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -138,6 +138,16 @@ public class Pose2d implements Interpolatable<Pose2d> {
     return times(1.0 / scalar);
   }
 
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d rotateBy(Rotation2d other) {
+    return new Pose2d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
   /**
    * Transforms the pose by the given transformation and returns the new pose. See + operator for
    * the matrix multiplication performed.

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -150,6 +150,16 @@ public class Pose3d implements Interpolatable<Pose3d> {
     return times(1.0 / scalar);
   }
 
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose3d rotateBy(Rotation3d other) {
+    return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
   /**
    * Transforms the pose by the given transformation and returns the new pose.
    *