Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Replace Speeds with Velocities (#8479)

Browse Source 
I left "free speed" alone since that's the technical term for it. In
general, velocity is a vector quantity, and speed is a magnitude (i.e.,
a strictly positive value).

This PR also replaces the speed verbiage in MotorController with duty
cycle.

Fixes #8423.
This commit is contained in:
Tyler Veness
2026-03-06 14:19:15 -08:00
committed by GitHub
parent 1e39f39128
commit 9bd9656871
594 changed files with 8073 additions and 7875 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp
View File

@@ -26,8 +26,8 @@ void testFollowTrajectory(
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -86,12 +86,12 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
leftDistance += wheelVelocities.left * dt;
rightDistance += wheelVelocities.right * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -181,8 +181,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -226,8 +226,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

20
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -26,8 +26,8 @@ void testFollowTrajectory(
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -85,12 +85,12 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
leftDistance += wheelVelocities.left * dt;
rightDistance += wheelVelocities.right * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -174,8 +174,8 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -218,8 +218,8 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

24
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp
View File

@@ -21,8 +21,8 @@ void testFollowTrajectory(
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -79,14 +79,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -175,8 +175,8 @@ TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -220,8 +220,8 @@ TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

24
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -21,8 +21,8 @@ void testFollowTrajectory(
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -78,14 +78,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -169,8 +169,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -214,8 +214,8 @@ TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

21
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
View File

@@ -22,8 +22,8 @@ void testFollowTrajectory(
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator3d<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -81,13 +81,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
auto moduleVelocities =
kinematics.ToSwerveModuleVelocities(chassisVelocities);
for (size_t i = 0; i < 4; i++) {
positions[i].distance += moduleStates[i].speed * dt;
positions[i].angle = moduleStates[i].angle;
positions[i].distance += moduleVelocities[i].velocity * dt;
positions[i].angle = moduleVelocities[i].angle;
}
auto xhat = estimator.UpdateWithTime(
@@ -180,8 +181,8 @@ TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -229,8 +230,8 @@ TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

21
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -22,8 +22,8 @@ void testFollowTrajectory(
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -79,13 +79,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
auto moduleVelocities =
kinematics.ToSwerveModuleVelocities(chassisVelocities);
for (size_t i = 0; i < 4; i++) {
positions[i].distance += moduleStates[i].speed * dt;
positions[i].angle = moduleStates[i].angle;
positions[i].distance += moduleVelocities[i].velocity * dt;
positions[i].angle = moduleVelocities[i].angle;
}
auto xhat = estimator.UpdateWithTime(
@@ -173,8 +174,8 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -222,8 +223,8 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,