[wpimath] Add ChassisSpeeds::ToTwist2d() to ChassisSpeeds (#6634)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

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

@@ -11,6 +11,7 @@ import static edu.wpi.first.units.Units.Seconds;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto;
 import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct;
 import edu.wpi.first.units.Angle;
@@ -77,6 +78,19 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
     this(vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond));
   }
 
+  /**
+   * Creates a Twist2d from ChassisSpeeds.
+   *
+   * @param dtSeconds The duration of the timestep.
+   * @return Twist2d.
+   */
+  public Twist2d toTwist2d(double dtSeconds) {
+    return new Twist2d(
+        vxMetersPerSecond * dtSeconds,
+        vyMetersPerSecond * dtSeconds,
+        omegaRadiansPerSecond * dtSeconds);
+  }
+
   /**
    * Discretizes a continuous-time chassis speed.
    *

wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h

@@ -38,6 +38,17 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
    */
   units::radians_per_second_t omega = 0_rad_per_s;
 
+  /**
+   * Creates a Twist2d from ChassisSpeeds.
+   *
+   * @param dt The duration of the timestep.
+   *
+   * @return Twist2d.
+   */
+  Twist2d ToTwist2d(units::second_t dt) const {
+    return Twist2d{vx * dt, vy * dt, omega * dt};
+  }
+
   /**
    * Disretizes a continuous-time chassis speed.
    *