[wpimath] Replace Speeds with Velocities (#8479)

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.

wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp

@@ -10,7 +10,7 @@
 
 using namespace wpi::math;
 
-ChassisSpeeds LTVUnicycleController::Calculate(
+ChassisVelocities LTVUnicycleController::Calculate(
     const Pose2d& currentPose, const Pose2d& poseRef,
     wpi::units::meters_per_second_t linearVelocityRef,
     wpi::units::radians_per_second_t angularVelocityRef) {
@@ -48,7 +48,7 @@ ChassisSpeeds LTVUnicycleController::Calculate(
   //     [0  0  0]              [0  1]
 
   if (!m_enabled) {
-    return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
+    return ChassisVelocities{linearVelocityRef, 0_mps, angularVelocityRef};
   }
 
   // The DARE is ill-conditioned if the velocity is close to zero, so don't
@@ -78,7 +78,7 @@ ChassisSpeeds LTVUnicycleController::Calculate(
                     m_poseError.Rotation().Radians().value()};
   Eigen::Vector2d u = K * e;
 
-  return ChassisSpeeds{
+  return ChassisVelocities{
       linearVelocityRef + wpi::units::meters_per_second_t{u(0)}, 0_mps,
       angularVelocityRef + wpi::units::radians_per_second_t{u(1)}};
 }

wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp

@@ -6,8 +6,8 @@
 
 using namespace wpi::math;
 
-MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
-    const ChassisSpeeds& chassisSpeeds,
+MecanumDriveWheelVelocities MecanumDriveKinematics::ToWheelVelocities(
+    const ChassisVelocities& chassisVelocities,
     const Translation2d& centerOfRotation) const {
   // We have a new center of rotation. We need to compute the matrix again.
   if (centerOfRotation != m_previousCoR) {
@@ -21,32 +21,33 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
     m_previousCoR = centerOfRotation;
   }
 
-  Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
-                                      chassisSpeeds.vy.value(),
-                                      chassisSpeeds.omega.value()};
+  Eigen::Vector3d chassisVelocitiesVector{chassisVelocities.vx.value(),
+                                          chassisVelocities.vy.value(),
+                                          chassisVelocities.omega.value()};
 
-  Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector;
+  Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisVelocitiesVector;
 
-  MecanumDriveWheelSpeeds wheelSpeeds;
-  wheelSpeeds.frontLeft = wpi::units::meters_per_second_t{wheelsVector(0)};
-  wheelSpeeds.frontRight = wpi::units::meters_per_second_t{wheelsVector(1)};
-  wheelSpeeds.rearLeft = wpi::units::meters_per_second_t{wheelsVector(2)};
-  wheelSpeeds.rearRight = wpi::units::meters_per_second_t{wheelsVector(3)};
-  return wheelSpeeds;
+  MecanumDriveWheelVelocities wheelVelocities;
+  wheelVelocities.frontLeft = wpi::units::meters_per_second_t{wheelsVector(0)};
+  wheelVelocities.frontRight = wpi::units::meters_per_second_t{wheelsVector(1)};
+  wheelVelocities.rearLeft = wpi::units::meters_per_second_t{wheelsVector(2)};
+  wheelVelocities.rearRight = wpi::units::meters_per_second_t{wheelsVector(3)};
+  return wheelVelocities;
 }
 
-ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
-    const MecanumDriveWheelSpeeds& wheelSpeeds) const {
-  Eigen::Vector4d wheelSpeedsVector{
-      wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
-      wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
+ChassisVelocities MecanumDriveKinematics::ToChassisVelocities(
+    const MecanumDriveWheelVelocities& wheelVelocities) const {
+  Eigen::Vector4d wheelVelocitiesVector{
+      wheelVelocities.frontLeft.value(), wheelVelocities.frontRight.value(),
+      wheelVelocities.rearLeft.value(), wheelVelocities.rearRight.value()};
 
-  Eigen::Vector3d chassisSpeedsVector =
-      m_forwardKinematics.solve(wheelSpeedsVector);
+  Eigen::Vector3d chassisVelocitiesVector =
+      m_forwardKinematics.solve(wheelVelocitiesVector);
 
-  return {wpi::units::meters_per_second_t{chassisSpeedsVector(0)},  // NOLINT
-          wpi::units::meters_per_second_t{chassisSpeedsVector(1)},
-          wpi::units::radians_per_second_t{chassisSpeedsVector(2)}};
+  return {
+      wpi::units::meters_per_second_t{chassisVelocitiesVector(0)},  // NOLINT
+      wpi::units::meters_per_second_t{chassisVelocitiesVector(1)},
+      wpi::units::radians_per_second_t{chassisVelocitiesVector(2)}};
 }
 
 Twist2d MecanumDriveKinematics::ToTwist2d(

wpimath/src/main/native/cpp/kinematics/proto/ChassisVelocitiesProto.cpp

@@ -2,27 +2,27 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "wpi/math/kinematics/proto/ChassisSpeedsProto.hpp"
+#include "wpi/math/kinematics/proto/ChassisVelocitiesProto.hpp"
 
 #include "wpimath/protobuf/kinematics.npb.h"
 
-std::optional<wpi::math::ChassisSpeeds>
-wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Unpack(InputStream& stream) {
-  wpi_proto_ProtobufChassisSpeeds msg;
+std::optional<wpi::math::ChassisVelocities>
+wpi::util::Protobuf<wpi::math::ChassisVelocities>::Unpack(InputStream& stream) {
+  wpi_proto_ProtobufChassisVelocities msg;
   if (!stream.Decode(msg)) {
     return {};
   }
 
-  return wpi::math::ChassisSpeeds{
+  return wpi::math::ChassisVelocities{
       wpi::units::meters_per_second_t{msg.vx},
       wpi::units::meters_per_second_t{msg.vy},
       wpi::units::radians_per_second_t{msg.omega},
   };
 }
 
-bool wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Pack(
-    OutputStream& stream, const wpi::math::ChassisSpeeds& value) {
-  wpi_proto_ProtobufChassisSpeeds msg{
+bool wpi::util::Protobuf<wpi::math::ChassisVelocities>::Pack(
+    OutputStream& stream, const wpi::math::ChassisVelocities& value) {
+  wpi_proto_ProtobufChassisVelocities msg{
       .vx = value.vx.value(),
       .vy = value.vy.value(),
       .omega = value.omega.value(),

wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp

@@ -1,30 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include "wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp"
-
-#include "wpimath/protobuf/kinematics.npb.h"
-
-std::optional<wpi::math::DifferentialDriveWheelSpeeds> wpi::util::Protobuf<
-    wpi::math::DifferentialDriveWheelSpeeds>::Unpack(InputStream& stream) {
-  wpi_proto_ProtobufDifferentialDriveWheelSpeeds msg;
-  if (!stream.Decode(msg)) {
-    return {};
-  }
-
-  return wpi::math::DifferentialDriveWheelSpeeds{
-      wpi::units::meters_per_second_t{msg.left},
-      wpi::units::meters_per_second_t{msg.right},
-  };
-}
-
-bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds>::Pack(
-    OutputStream& stream,
-    const wpi::math::DifferentialDriveWheelSpeeds& value) {
-  wpi_proto_ProtobufDifferentialDriveWheelSpeeds msg{
-      .left = value.left.value(),
-      .right = value.right.value(),
-  };
-  return stream.Encode(msg);
-}

wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelVelocitiesProto.cpp

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/math/kinematics/proto/DifferentialDriveWheelVelocitiesProto.hpp"
+
+#include "wpimath/protobuf/kinematics.npb.h"
+
+std::optional<wpi::math::DifferentialDriveWheelVelocities> wpi::util::Protobuf<
+    wpi::math::DifferentialDriveWheelVelocities>::Unpack(InputStream& stream) {
+  wpi_proto_ProtobufDifferentialDriveWheelVelocities msg;
+  if (!stream.Decode(msg)) {
+    return {};
+  }
+
+  return wpi::math::DifferentialDriveWheelVelocities{
+      wpi::units::meters_per_second_t{msg.left},
+      wpi::units::meters_per_second_t{msg.right},
+  };
+}
+
+bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVelocities>::Pack(
+    OutputStream& stream,
+    const wpi::math::DifferentialDriveWheelVelocities& value) {
+  wpi_proto_ProtobufDifferentialDriveWheelVelocities msg{
+      .left = value.left.value(),
+      .right = value.right.value(),
+  };
+  return stream.Encode(msg);
+}

wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelVelocitiesProto.cpp

@@ -2,18 +2,18 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp"
+#include "wpi/math/kinematics/proto/MecanumDriveWheelVelocitiesProto.hpp"
 
 #include "wpimath/protobuf/kinematics.npb.h"
 
-std::optional<wpi::math::MecanumDriveWheelSpeeds> wpi::util::Protobuf<
-    wpi::math::MecanumDriveWheelSpeeds>::Unpack(InputStream& stream) {
-  wpi_proto_ProtobufMecanumDriveWheelSpeeds msg;
+std::optional<wpi::math::MecanumDriveWheelVelocities> wpi::util::Protobuf<
+    wpi::math::MecanumDriveWheelVelocities>::Unpack(InputStream& stream) {
+  wpi_proto_ProtobufMecanumDriveWheelVelocities msg;
   if (!stream.Decode(msg)) {
     return {};
   }
 
-  return wpi::math::MecanumDriveWheelSpeeds{
+  return wpi::math::MecanumDriveWheelVelocities{
       wpi::units::meters_per_second_t{msg.front_left},
       wpi::units::meters_per_second_t{msg.front_right},
       wpi::units::meters_per_second_t{msg.rear_left},
@@ -21,9 +21,9 @@ std::optional<wpi::math::MecanumDriveWheelSpeeds> wpi::util::Protobuf<
   };
 }
 
-bool wpi::util::Protobuf<wpi::math::MecanumDriveWheelSpeeds>::Pack(
-    OutputStream& stream, const wpi::math::MecanumDriveWheelSpeeds& value) {
-  wpi_proto_ProtobufMecanumDriveWheelSpeeds msg{
+bool wpi::util::Protobuf<wpi::math::MecanumDriveWheelVelocities>::Pack(
+    OutputStream& stream, const wpi::math::MecanumDriveWheelVelocities& value) {
+  wpi_proto_ProtobufMecanumDriveWheelVelocities msg{
       .front_left = value.frontLeft.value(),
       .front_right = value.frontRight.value(),
       .rear_left = value.rearLeft.value(),

wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleVelocityProto.cpp

@@ -2,16 +2,16 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "wpi/math/kinematics/proto/SwerveModuleStateProto.hpp"
+#include "wpi/math/kinematics/proto/SwerveModuleVelocityProto.hpp"
 
 #include "wpi/util/protobuf/ProtobufCallbacks.hpp"
 #include "wpimath/protobuf/kinematics.npb.h"
 
-std::optional<wpi::math::SwerveModuleState>
-wpi::util::Protobuf<wpi::math::SwerveModuleState>::Unpack(InputStream& stream) {
+std::optional<wpi::math::SwerveModuleVelocity> wpi::util::Protobuf<
+    wpi::math::SwerveModuleVelocity>::Unpack(InputStream& stream) {
   wpi::util::UnpackCallback<wpi::math::Rotation2d> angle;
-  wpi_proto_ProtobufSwerveModuleState msg{
-      .speed = 0,
+  wpi_proto_ProtobufSwerveModuleVelocity msg{
+      .velocity = 0,
       .angle = angle.Callback(),
   };
   if (!stream.Decode(msg)) {
@@ -24,17 +24,17 @@ wpi::util::Protobuf<wpi::math::SwerveModuleState>::Unpack(InputStream& stream) {
     return {};
   }
 
-  return wpi::math::SwerveModuleState{
-      wpi::units::meters_per_second_t{msg.speed},
+  return wpi::math::SwerveModuleVelocity{
+      wpi::units::meters_per_second_t{msg.velocity},
       iangle[0],
   };
 }
 
-bool wpi::util::Protobuf<wpi::math::SwerveModuleState>::Pack(
-    OutputStream& stream, const wpi::math::SwerveModuleState& value) {
+bool wpi::util::Protobuf<wpi::math::SwerveModuleVelocity>::Pack(
+    OutputStream& stream, const wpi::math::SwerveModuleVelocity& value) {
   wpi::util::PackCallback angle{&value.angle};
-  wpi_proto_ProtobufSwerveModuleState msg{
-      .speed = value.speed.value(),
+  wpi_proto_ProtobufSwerveModuleVelocity msg{
+      .velocity = value.velocity.value(),
       .angle = angle.Callback(),
   };
   return stream.Encode(msg);

wpimath/src/main/native/cpp/kinematics/struct/ChassisVelocitiesStruct.cpp

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "wpi/math/kinematics/struct/ChassisSpeedsStruct.hpp"
+#include "wpi/math/kinematics/struct/ChassisVelocitiesStruct.hpp"
 
 namespace {
 constexpr size_t kVxOff = 0;
@@ -10,10 +10,10 @@ constexpr size_t kVyOff = kVxOff + 8;
 constexpr size_t kOmegaOff = kVyOff + 8;
 }  // namespace
 
-using StructType = wpi::util::Struct<wpi::math::ChassisSpeeds>;
+using StructType = wpi::util::Struct<wpi::math::ChassisVelocities>;
 
-wpi::math::ChassisSpeeds StructType::Unpack(std::span<const uint8_t> data) {
-  return wpi::math::ChassisSpeeds{
+wpi::math::ChassisVelocities StructType::Unpack(std::span<const uint8_t> data) {
+  return wpi::math::ChassisVelocities{
       wpi::units::meters_per_second_t{
           wpi::util::UnpackStruct<double, kVxOff>(data)},
       wpi::units::meters_per_second_t{
@@ -24,7 +24,7 @@ wpi::math::ChassisSpeeds StructType::Unpack(std::span<const uint8_t> data) {
 }
 
 void StructType::Pack(std::span<uint8_t> data,
-                      const wpi::math::ChassisSpeeds& value) {
+                      const wpi::math::ChassisVelocities& value) {
   wpi::util::PackStruct<kVxOff>(data, value.vx.value());
   wpi::util::PackStruct<kVyOff>(data, value.vy.value());
   wpi::util::PackStruct<kOmegaOff>(data, value.omega.value());

wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelVelocitiesStruct.cpp

@@ -2,18 +2,19 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "wpi/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.hpp"
+#include "wpi/math/kinematics/struct/DifferentialDriveWheelVelocitiesStruct.hpp"
 
 namespace {
 constexpr size_t kLeftOff = 0;
 constexpr size_t kRightOff = kLeftOff + 8;
 }  // namespace
 
-using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds>;
+using StructType =
+    wpi::util::Struct<wpi::math::DifferentialDriveWheelVelocities>;
 
-wpi::math::DifferentialDriveWheelSpeeds StructType::Unpack(
+wpi::math::DifferentialDriveWheelVelocities StructType::Unpack(
     std::span<const uint8_t> data) {
-  return wpi::math::DifferentialDriveWheelSpeeds{
+  return wpi::math::DifferentialDriveWheelVelocities{
       wpi::units::meters_per_second_t{
           wpi::util::UnpackStruct<double, kLeftOff>(data)},
       wpi::units::meters_per_second_t{
@@ -21,8 +22,9 @@ wpi::math::DifferentialDriveWheelSpeeds StructType::Unpack(
   };
 }
 
-void StructType::Pack(std::span<uint8_t> data,
-                      const wpi::math::DifferentialDriveWheelSpeeds& value) {
+void StructType::Pack(
+    std::span<uint8_t> data,
+    const wpi::math::DifferentialDriveWheelVelocities& value) {
   wpi::util::PackStruct<kLeftOff>(data, value.left.value());
   wpi::util::PackStruct<kRightOff>(data, value.right.value());
 }

wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelVelocitiesStruct.cpp

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "wpi/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.hpp"
+#include "wpi/math/kinematics/struct/MecanumDriveWheelVelocitiesStruct.hpp"
 
 namespace {
 constexpr size_t kFrontLeftOff = 0;
@@ -11,11 +11,11 @@ constexpr size_t kRearLeftOff = kFrontRightOff + 8;
 constexpr size_t kRearRightOff = kRearLeftOff + 8;
 }  // namespace
 
-using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds>;
+using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelVelocities>;
 
-wpi::math::MecanumDriveWheelSpeeds StructType::Unpack(
+wpi::math::MecanumDriveWheelVelocities StructType::Unpack(
     std::span<const uint8_t> data) {
-  return wpi::math::MecanumDriveWheelSpeeds{
+  return wpi::math::MecanumDriveWheelVelocities{
       wpi::units::meters_per_second_t{
           wpi::util::UnpackStruct<double, kFrontLeftOff>(data)},
       wpi::units::meters_per_second_t{
@@ -28,7 +28,7 @@ wpi::math::MecanumDriveWheelSpeeds StructType::Unpack(
 }
 
 void StructType::Pack(std::span<uint8_t> data,
-                      const wpi::math::MecanumDriveWheelSpeeds& value) {
+                      const wpi::math::MecanumDriveWheelVelocities& value) {
   wpi::util::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
   wpi::util::PackStruct<kFrontRightOff>(data, value.frontRight.value());
   wpi::util::PackStruct<kRearLeftOff>(data, value.rearLeft.value());

wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp

@@ -1,26 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#include "wpi/math/kinematics/struct/SwerveModuleStateStruct.hpp"
-
-namespace {
-constexpr size_t kSpeedOff = 0;
-constexpr size_t kAngleOff = kSpeedOff + 8;
-}  // namespace
-
-using StructType = wpi::util::Struct<wpi::math::SwerveModuleState>;
-
-wpi::math::SwerveModuleState StructType::Unpack(std::span<const uint8_t> data) {
-  return wpi::math::SwerveModuleState{
-      wpi::units::meters_per_second_t{
-          wpi::util::UnpackStruct<double, kSpeedOff>(data)},
-      wpi::util::UnpackStruct<wpi::math::Rotation2d, kAngleOff>(data),
-  };
-}
-
-void StructType::Pack(std::span<uint8_t> data,
-                      const wpi::math::SwerveModuleState& value) {
-  wpi::util::PackStruct<kSpeedOff>(data, value.speed.value());
-  wpi::util::PackStruct<kAngleOff>(data, value.angle);
-}

wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleVelocityStruct.cpp

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/math/kinematics/struct/SwerveModuleVelocityStruct.hpp"
+
+namespace {
+constexpr size_t kVelocityOff = 0;
+constexpr size_t kAngleOff = kVelocityOff + 8;
+}  // namespace
+
+using StructType = wpi::util::Struct<wpi::math::SwerveModuleVelocity>;
+
+wpi::math::SwerveModuleVelocity StructType::Unpack(
+    std::span<const uint8_t> data) {
+  return wpi::math::SwerveModuleVelocity{
+      wpi::units::meters_per_second_t{
+          wpi::util::UnpackStruct<double, kVelocityOff>(data)},
+      wpi::util::UnpackStruct<wpi::math::Rotation2d, kAngleOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const wpi::math::SwerveModuleVelocity& value) {
+  wpi::util::PackStruct<kVelocityOff>(data, value.velocity.value());
+  wpi::util::PackStruct<kAngleOff>(data, value.angle);
+}