[wpimath] Add Pose3d(Pose2d) constructor (#4485)

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

@@ -48,6 +48,16 @@ public class Pose3d implements Interpolatable<Pose3d> {
     m_rotation = rotation;
   }
 
+  /**
+   * Constructs a 3D pose from a 2D pose in the X-Y plane.
+   *
+   * @param pose The 2D pose.
+   */
+  public Pose3d(Pose2d pose) {
+    m_translation = new Translation3d(pose.getX(), pose.getY(), 0.0);
+    m_rotation = new Rotation3d(0.0, 0.0, pose.getRotation().getRadians());
+  }
+
   /**
    * Transforms the pose by the given transformation and returns the new transformed pose.
    *

wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -37,6 +37,10 @@ Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
                Rotation3d rotation)
     : m_translation(x, y, z), m_rotation(std::move(rotation)) {}
 
+Pose3d::Pose3d(const Pose2d& pose)
+    : m_translation(pose.X(), pose.Y(), 0_m),
+      m_rotation(0_rad, 0_rad, pose.Rotation().Radians()) {}
+
 Pose3d Pose3d::operator+(const Transform3d& other) const {
   return TransformBy(other);
 }

wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -43,6 +43,13 @@ class WPILIB_DLLEXPORT Pose3d {
   Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
          Rotation3d rotation);
 
+  /**
+   * Constructs a 3D pose from a 2D pose in the X-Y plane.
+   *
+   * @param pose The 2D pose.
+   */
+  explicit Pose3d(const Pose2d& pose);
+
   /**
    * Transforms the pose by the given transformation and returns the new
    * transformed pose.