The difference between applying a chassis speed for a discrete timestep vs. continuously is - * that applying for a discrete timestep is just scaling the velocity components by the time and - * adding, while when applying continuously the changes to the heading affect the direction the - * translational components are applied to relative to the field. + *
This function converts a continous-time chassis speed into a discrete-time one such that + * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + * along the y-axis, and omega * dt around the z-axis). + * + *
This is useful for compensating for translational skew when translating and rotating a + * swerve drivetrain. * * @param vxMetersPerSecond Forward velocity. * @param vyMetersPerSecond Sideways velocity. * @param omegaRadiansPerSecond Angular velocity. * @param dtSeconds The duration of the timestep the speeds should be applied for. - * @return ChassisSpeeds that can be applied continuously to produce the discrete chassis speeds. + * @return Discretized ChassisSpeeds. */ - public static ChassisSpeeds fromDiscreteSpeeds( + public static ChassisSpeeds discretize( double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond, @@ -72,22 +75,25 @@ public class ChassisSpeeds { } /** - * Converts from a chassis speed for a discrete timestep into chassis speed for continuous time. + * Discretizes a continuous-time chassis speed. * - *
The difference between applying a chassis speed for a discrete timestep vs. continuously is - * that applying for a discrete timestep is just scaling the velocity components by the time and - * adding, while when applying continuously the changes to the heading affect the direction the - * translational components are applied to relative to the field. + *
This function converts a continous-time chassis speed into a discrete-time one such that + * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + * along the y-axis, and omega * dt around the z-axis). * - * @param discreteSpeeds The speeds for a discrete timestep. + *
This is useful for compensating for translational skew when translating and rotating a + * swerve drivetrain. + * + * @param continuousSpeeds The continuous speeds. * @param dtSeconds The duration of the timestep the speeds should be applied for. - * @return ChassisSpeeds that can be applied continuously to produce the discrete chassis speeds. + * @return Discretized ChassisSpeeds. */ - public static ChassisSpeeds fromDiscreteSpeeds(ChassisSpeeds discreteSpeeds, double dtSeconds) { - return fromDiscreteSpeeds( - discreteSpeeds.vxMetersPerSecond, - discreteSpeeds.vyMetersPerSecond, - discreteSpeeds.omegaRadiansPerSecond, + public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) { + return discretize( + continuousSpeeds.vxMetersPerSecond, + continuousSpeeds.vyMetersPerSecond, + continuousSpeeds.omegaRadiansPerSecond, dtSeconds); } diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index 8b65eb52af..bfee34ea94 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -39,52 +39,54 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { units::radians_per_second_t omega = 0_rad_per_s; /** - * Converts from a chassis speed for a discrete timestep into chassis speed - * for continuous time. + * Disretizes a continuous-time chassis speed. * - * The difference between applying a chassis speed for a discrete timestep vs. - * continuously is that applying for a discrete timestep is just scaling the - * velocity components by the time and adding, while when applying - * continuously the changes to the heading affect the direction the - * translational components are applied to relative to the field. + * This function converts a continous-time chassis speed into a discrete-time + * one such that when the discrete-time chassis speed is applied for one + * timestep, the robot moves as if the velocity components are independent + * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the + * y-axis, and omega * dt around the z-axis). + * + * This is useful for compensating for translational skew when translating and + * rotating a swerve drivetrain. * * @param vx Forward velocity. * @param vy Sideways velocity. * @param omega Angular velocity. * @param dt The duration of the timestep the speeds should be applied for. * - * @return ChassisSpeeds that can be applied continuously to produce the - * discrete ChassisSpeeds. + * @return Discretized ChassisSpeeds. */ - static ChassisSpeeds FromDiscreteSpeeds(units::meters_per_second_t vx, - units::meters_per_second_t vy, - units::radians_per_second_t omega, - units::second_t dt) { + static ChassisSpeeds Discretize(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega, + units::second_t dt) { Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt}; auto twist = Pose2d{}.Log(desiredDeltaPose); return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt}; } /** - * Converts from a chassis speed for a discrete timestep into chassis speed - * for continuous time. + * Disretizes a continuous-time chassis speed. * - * The difference between applying a chassis speed for a discrete timestep vs. - * continuously is that applying for a discrete timestep is just scaling the - * velocity components by the time and adding, while when applying - * continuously the changes to the heading affect the direction the - * translational components are applied to relative to the field. + * This function converts a continous-time chassis speed into a discrete-time + * one such that when the discrete-time chassis speed is applied for one + * timestep, the robot moves as if the velocity components are independent + * (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the + * y-axis, and omega * dt around the z-axis). * - * @param discreteSpeeds The speeds for a discrete timestep. + * This is useful for compensating for translational skew when translating and + * rotating a swerve drivetrain. + * + * @param continuousSpeeds The continuous speeds. * @param dt The duration of the timestep the speeds should be applied for. * - * @return ChassisSpeeds that can be applied continuously to produce the - * discrete ChassisSpeeds. + * @return Discretized ChassisSpeeds. */ - static ChassisSpeeds FromDiscreteSpeeds(const ChassisSpeeds& discreteSpeeds, - units::second_t dt) { - return FromDiscreteSpeeds(discreteSpeeds.vx, discreteSpeeds.vy, - discreteSpeeds.omega, dt); + static ChassisSpeeds Discretize(const ChassisSpeeds& continuousSpeeds, + units::second_t dt) { + return Discretize(continuousSpeeds.vx, continuousSpeeds.vy, + continuousSpeeds.omega, dt); } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index 26a1a7d533..034bebda21 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -16,20 +16,23 @@ class ChassisSpeedsTest { private static final double kEpsilon = 1E-9; @Test - void testVeeringCorrection() { - final var duration = 1.0; // duration of observation - final var dt = 0.01; // time increment for simulation + void testDiscretize() { final var target = new ChassisSpeeds(1.0, 0.0, 0.5); - final var speeds = ChassisSpeeds.fromDiscreteSpeeds(target, duration); + final var duration = 1.0; + final var dt = 0.01; + + final var speeds = ChassisSpeeds.discretize(target, duration); final var twist = new Twist2d( speeds.vxMetersPerSecond * dt, speeds.vyMetersPerSecond * dt, speeds.omegaRadiansPerSecond * dt); + var pose = new Pose2d(); for (double time = 0; time < duration; time += dt) { pose = pose.exp(twist); } + final var result = pose; // For lambda capture assertAll( () -> assertEquals(target.vxMetersPerSecond * duration, result.getX(), kEpsilon), @@ -42,7 +45,7 @@ class ChassisSpeedsTest { } @Test - void testFieldRelativeConstruction() { + void testFromFieldRelativeSpeeds() { final var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0)); diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp index ae1e0d0aa3..bab3ee558e 100644 --- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp @@ -8,7 +8,26 @@ static constexpr double kEpsilon = 1E-9; -TEST(ChassisSpeedsTest, FieldRelativeConstruction) { +TEST(ChassisSpeedsTest, Discretize) { + constexpr frc::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s}; + constexpr units::second_t duration = 1_s; + constexpr units::second_t dt = 10_ms; + + const auto speeds = frc::ChassisSpeeds::Discretize(target, duration); + const frc::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt}; + + frc::Pose2d pose; + for (units::second_t time = 0_s; time < duration; time += dt) { + pose = pose.Exp(twist); + } + + EXPECT_NEAR((target.vx * duration).value(), pose.X().value(), kEpsilon); + EXPECT_NEAR((target.vy * duration).value(), pose.Y().value(), kEpsilon); + EXPECT_NEAR((target.omega * duration).value(), + pose.Rotation().Radians().value(), kEpsilon); +} + +TEST(ChassisSpeedsTest, FromFieldRelativeSpeeds) { const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds( 1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg);