Merge branch 'main' into 2027

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

@@ -238,6 +238,17 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
     return new Pose2d(transform.getTranslation(), transform.getRotation());
   }
 
+  /**
+   * Rotates the current pose around a point in 2D space.
+   *
+   * @param point The point in 2D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   * @return The new rotated pose.
+   */
+  public Pose2d rotateAround(Translation2d point, Rotation2d rot) {
+    return new Pose2d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
+  }
+
   /**
    * Obtain a new Pose2d from a (constant curvature) velocity.
    *

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

@@ -271,6 +271,17 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
     return new Pose3d(transform.getTranslation(), transform.getRotation());
   }
 
+  /**
+   * Rotates the current pose around a point in 3D space.
+   *
+   * @param point The point in 3D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   * @return The new rotated pose.
+   */
+  public Pose3d rotateAround(Translation3d point, Rotation3d rot) {
+    return new Pose3d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
+  }
+
   /**
    * Obtain a new Pose3d from a (constant curvature) velocity.
    *

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

@@ -110,11 +110,11 @@ public class Rotation2d
   public Rotation2d(double x, double y) {
     double magnitude = Math.hypot(x, y);
     if (magnitude > 1e-6) {
-      m_sin = y / magnitude;
       m_cos = x / magnitude;
+      m_sin = y / magnitude;
     } else {
-      m_sin = 0.0;
       m_cos = 1.0;
+      m_sin = 0.0;
       MathSharedStore.reportError(
           "x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace());
     }

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

@@ -216,6 +216,17 @@ public class Translation3d
     return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
   }
 
+  /**
+   * Rotates this translation around another translation in 3D space.
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation3d rotateAround(Translation3d other, Rotation3d rot) {
+    return this.minus(other).rotateBy(rot).plus(other);
+  }
+
   /**
    * Returns a Translation2d representing this Translation3d projected into the X-Y plane.
    *

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java

@@ -11,11 +11,15 @@ import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto;
 import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct;
 import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
 
 /** Represents the wheel positions for a differential drive drivetrain. */
 public class DifferentialDriveWheelPositions
-    implements Interpolatable<DifferentialDriveWheelPositions> {
+    implements StructSerializable,
+        ProtobufSerializable,
+        Interpolatable<DifferentialDriveWheelPositions> {
   /** Distance measured by the left side. */
   public double leftMeters;
 

wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java

@@ -20,7 +20,7 @@ public final class LinearSystemId {
 
   /**
    * Create a state-space model of an elevator system. The states of the system are [position,
-   * velocity]ᵀ, inputs are [voltage], and outputs are [position].
+   * velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
    *
    * @param motor The motor (or gearbox) attached to the carriage.
    * @param massKg The mass of the elevator carriage, in kilograms.
@@ -88,8 +88,8 @@ public final class LinearSystemId {
 
   /**
    * Create a state-space model of a DC motor system. The states of the system are [angular
-   * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
-   * velocity].
+   * position, angular velocity]ᵀ, inputs are [voltage], and outputs are [angular position, angular
+   * velocity]ᵀ.
    *
    * @param motor The motor (or gearbox) attached to system.
    * @param JKgMetersSquared The moment of inertia J of the DC motor.
@@ -125,7 +125,7 @@ public final class LinearSystemId {
   /**
    * Create a state-space model of a DC motor system from its kV (volts/(unit/sec)) and kA
    * (volts/(unit/sec²)). These constants can be found using SysId. the states of the system are
-   * [position, velocity], inputs are [voltage], and outputs are [position].
+   * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
    *
    * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
    * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
@@ -211,7 +211,7 @@ public final class LinearSystemId {
 
   /**
    * Create a state-space model of a single jointed arm system. The states of the system are [angle,
-   * angular velocity], inputs are [voltage], and outputs are [angle].
+   * angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ.
    *
    * @param motor The motor (or gearbox) attached to the arm.
    * @param JKgSquaredMeters The moment of inertia J of the arm.
@@ -279,7 +279,7 @@ public final class LinearSystemId {
   /**
    * Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA
    * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
-   * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
+   * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
    *
    * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
    * {@link edu.wpi.first.math.util.Units} class for converting between unit types.