Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Rename ChassisSpeeds.fromDiscreteSpeeds() to discretize() (#5616)

Browse Source 
This better reflects what's actually going on mathematically.
This commit is contained in:
Tyler Veness
2023-09-08 20:14:59 -07:00
committed by GitHub
parent 8e05983a4a
commit a9ab08f48b
14 changed files with 101 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -51,12 +51,11 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
}

11
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -48,12 +48,11 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
}

2
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
View File

@@ -9,7 +9,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states =
m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::FromDiscreteSpeeds(
m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},

4
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -53,8 +53,8 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states = kDriveKinematics.ToSwerveModuleStates(
frc::ChassisSpeeds::FromDiscreteSpeeds(
auto states =
kDriveKinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -13,7 +13,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states =
m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::FromDiscreteSpeeds(
m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -132,7 +132,7 @@ public class Drivetrain {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
ChassisSpeeds.fromDiscreteSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -144,7 +144,7 @@ public class Drivetrain {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
ChassisSpeeds.fromDiscreteSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View File

@@ -59,7 +59,7 @@ public class Drivetrain {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
ChassisSpeeds.fromDiscreteSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -120,7 +120,7 @@ public class DriveSubsystem extends Subsystem {
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
var swerveModuleStates =
DriveConstants.kDriveKinematics.toSwerveModuleStates(
ChassisSpeeds.fromDiscreteSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -68,7 +68,7 @@ public class Drivetrain {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
ChassisSpeeds.fromDiscreteSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())

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

@@ -44,20 +44,23 @@ public class ChassisSpeeds {
}
/**
* Converts from a chassis speed for a discrete timestep into chassis speed for continuous time.
* Discretizes a continuous-time chassis speed.
*
* <p>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.
* <p>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).
*
* <p>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.
*
* <p>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.
* <p>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.
* <p>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);
}

56
wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
View File

@@ -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);
}
/**

13
wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
View File

@@ -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));

21
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
View File

@@ -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);