diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 252ab1b594..608c735e9e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/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. * diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index f8e8446588..d693b47068 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/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. *