Merge branch 'main' into development

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java

@@ -7,6 +7,8 @@ package edu.wpi.first.math.kinematics;
 import static edu.wpi.first.units.Units.Meters;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto;
+import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import java.util.Objects;
@@ -20,6 +22,14 @@ public class DifferentialDriveWheelPositions
   /** Distance measured by the right side. */
   public double rightMeters;
 
+  /** DifferentialDriveWheelPostions struct for serialization. */
+  public static final DifferentialDriveWheelPositionsStruct struct =
+      new DifferentialDriveWheelPositionsStruct();
+
+  /** DifferentialDriveWheelPostions struct for serialization. */
+  public static final DifferentialDriveWheelPositionsProto proto =
+      new DifferentialDriveWheelPositionsProto();
+
   /**
    * Constructs a DifferentialDriveWheelPositions.
    *

wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java

@@ -0,0 +1,40 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelPositions;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class DifferentialDriveWheelPositionsProto
+    implements Protobuf<DifferentialDriveWheelPositions, ProtobufDifferentialDriveWheelPositions> {
+  @Override
+  public Class<DifferentialDriveWheelPositions> getTypeClass() {
+    return DifferentialDriveWheelPositions.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDifferentialDriveWheelPositions.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDifferentialDriveWheelPositions createMessage() {
+    return ProtobufDifferentialDriveWheelPositions.newInstance();
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions unpack(ProtobufDifferentialDriveWheelPositions msg) {
+    return new DifferentialDriveWheelPositions(msg.getLeft(), msg.getRight());
+  }
+
+  @Override
+  public void pack(
+      ProtobufDifferentialDriveWheelPositions msg, DifferentialDriveWheelPositions value) {
+    msg.setLeft(value.leftMeters);
+    msg.setRight(value.rightMeters);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java

@@ -0,0 +1,45 @@
+// 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.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class DifferentialDriveWheelPositionsStruct
+    implements Struct<DifferentialDriveWheelPositions> {
+  @Override
+  public Class<DifferentialDriveWheelPositions> getTypeClass() {
+    return DifferentialDriveWheelPositions.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:DifferentialDriveWheelPositions";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 2;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double left;double right";
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions unpack(ByteBuffer bb) {
+    double left = bb.getDouble();
+    double right = bb.getDouble();
+    return new DifferentialDriveWheelPositions(left, right);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DifferentialDriveWheelPositions value) {
+    bb.putDouble(value.leftMeters);
+    bb.putDouble(value.rightMeters);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java

@@ -135,14 +135,14 @@ public final class LinearSystemId {
    * <p>u = K_v v + K_a a
    *
    * @param kV The velocity gain, in volts/(unit/sec)
-   * @param kA The acceleration gain, in volts/(unit/sec^2)
+   * @param kA The acceleration gain, in volts/(unit/sec²)
    * @return A LinearSystem representing the given characterized constants.
-   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
+   * @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
    * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   public static LinearSystem<N2, N1, N2> createDCMotorSystem(double kV, double kA) {
-    if (kV <= 0.0) {
-      throw new IllegalArgumentException("Kv must be greater than zero.");
+    if (kV < 0.0) {
+      throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
     }
     if (kA <= 0.0) {
       throw new IllegalArgumentException("Ka must be greater than zero.");
@@ -256,14 +256,14 @@ public final class LinearSystemId {
    * <p>u = K_v v + K_a a
    *
    * @param kV The velocity gain, in volts/(unit/sec)
-   * @param kA The acceleration gain, in volts/(unit/sec^2)
+   * @param kA The acceleration gain, in volts/(unit/sec²)
    * @return A LinearSystem representing the given characterized constants.
-   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
+   * @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
    * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
     if (kV <= 0.0) {
-      throw new IllegalArgumentException("Kv must be greater than zero.");
+      throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
     }
     if (kA <= 0.0) {
       throw new IllegalArgumentException("Ka must be greater than zero.");
@@ -291,12 +291,12 @@ public final class LinearSystemId {
    * @param kV The velocity gain, in volts/(unit/sec)
    * @param kA The acceleration gain, in volts/(unit/sec²)
    * @return A LinearSystem representing the given characterized constants.
-   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
+   * @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
    * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
     if (kV <= 0.0) {
-      throw new IllegalArgumentException("Kv must be greater than zero.");
+      throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
     }
     if (kA <= 0.0) {
       throw new IllegalArgumentException("Ka must be greater than zero.");

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

@@ -0,0 +1,34 @@
+// 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 "frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<
+    frc::DifferentialDriveWheelPositions>::New(google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufDifferentialDriveWheelPositions>(arena);
+}
+
+frc::DifferentialDriveWheelPositions
+wpi::Protobuf<frc::DifferentialDriveWheelPositions>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m =
+      static_cast<const wpi::proto::ProtobufDifferentialDriveWheelPositions*>(
+          &msg);
+  return frc::DifferentialDriveWheelPositions{
+      units::meter_t{m->left()},
+      units::meter_t{m->right()},
+  };
+}
+
+void wpi::Protobuf<frc::DifferentialDriveWheelPositions>::Pack(
+    google::protobuf::Message* msg,
+    const frc::DifferentialDriveWheelPositions& value) {
+  auto m =
+      static_cast<wpi::proto::ProtobufDifferentialDriveWheelPositions*>(msg);
+  m->set_left(value.left.value());
+  m->set_right(value.right.value());
+}

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

@@ -0,0 +1,26 @@
+// 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 "frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h"
+
+namespace {
+constexpr size_t kLeftOff = 0;
+constexpr size_t kRightOff = kLeftOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelPositions>;
+
+frc::DifferentialDriveWheelPositions StructType::Unpack(
+    std::span<const uint8_t> data) {
+  return frc::DifferentialDriveWheelPositions{
+      units::meter_t{wpi::UnpackStruct<double, kLeftOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kRightOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::DifferentialDriveWheelPositions& value) {
+  wpi::PackStruct<kLeftOff>(data, value.left.value());
+  wpi::PackStruct<kRightOff>(data, value.right.value());
+}

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h

@@ -49,3 +49,6 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
   }
 };
 }  // namespace frc
+
+#include "frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h"
+#include "frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h"

wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h

@@ -0,0 +1,19 @@
+// 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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveWheelPositions> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::DifferentialDriveWheelPositions Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::DifferentialDriveWheelPositions& value);
+};

wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h

@@ -0,0 +1,28 @@
+// 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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelPositions> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:DifferentialDriveWheelPositions";
+  }
+  static constexpr size_t GetSize() { return 16; }
+  static constexpr std::string_view GetSchema() {
+    return "double left;double right";
+  }
+
+  static frc::DifferentialDriveWheelPositions Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::DifferentialDriveWheelPositions& value);
+};
+
+static_assert(wpi::StructSerializable<frc::DifferentialDriveWheelPositions>);

wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h

@@ -79,7 +79,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
    *
    * @param kV The velocity gain, in volts/(unit/sec).
    * @param kA The acceleration gain, in volts/(unit/sec²).
-   * @throws std::domain_error if kV <= 0 or kA <= 0.
+   * @throws std::domain_error if kV < 0 or kA <= 0.
    * @see <a
    * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
@@ -89,8 +89,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
   static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
-    if (kV <= decltype(kV){0}) {
-      throw std::domain_error("Kv must be greater than zero.");
+    if (kV < decltype(kV){0}) {
+      throw std::domain_error("Kv must be greater than or equal to zero.");
     }
     if (kA <= decltype(kA){0}) {
       throw std::domain_error("Ka must be greater than zero.");
@@ -122,7 +122,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param kV The velocity gain, in volts/(unit/sec).
    * @param kA The acceleration gain, in volts/(unit/sec²).
    *
-   * @throws std::domain_error if kV <= 0 or kA <= 0.
+   * @throws std::domain_error if kV < 0 or kA <= 0.
    * @see <a
    * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
@@ -132,8 +132,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
   static LinearSystem<2, 1, 1> IdentifyPositionSystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
-    if (kV <= decltype(kV){0}) {
-      throw std::domain_error("Kv must be greater than zero.");
+    if (kV < decltype(kV){0}) {
+      throw std::domain_error("Kv must be greater than or equal to zero.");
     }
     if (kA <= decltype(kA){0}) {
       throw std::domain_error("Ka must be greater than zero.");
@@ -251,7 +251,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param kV The velocity gain, in volts/(unit/sec).
    * @param kA The acceleration gain, in volts/(unit/sec²).
    *
-   * @throws std::domain_error if kV <= 0 or kA <= 0.
+   * @throws std::domain_error if kV < 0 or kA <= 0.
    */
   template <typename Distance>
     requires std::same_as<units::meter, Distance> ||
@@ -259,8 +259,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
   static LinearSystem<2, 1, 2> DCMotorSystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
-    if (kV <= decltype(kV){0}) {
-      throw std::domain_error("Kv must be greater than zero.");
+    if (kV < decltype(kV){0}) {
+      throw std::domain_error("Kv must be greater than or equal to zero.");
     }
     if (kA <= decltype(kA){0}) {
       throw std::domain_error("Ka must be greater than zero.");

wpimath/src/main/proto/kinematics.proto

@@ -21,6 +21,11 @@ message ProtobufDifferentialDriveWheelSpeeds {
   double right = 2;
 }
 
+message ProtobufDifferentialDriveWheelPositions {
+  double left = 1;
+  double right = 2;
+}
+
 message ProtobufMecanumDriveKinematics {
   ProtobufTranslation2d front_left = 1;
   ProtobufTranslation2d front_right = 2;