Add struct implementations for LinearSystem

wpimath/src/main/java/edu/wpi/first/math/struct/MatrixStruct.java

@@ -33,9 +33,19 @@ public Class<Matrix<R, C>> getTypeClass() {
     return clazz;
   }
 
+  /**
+   * Returns the type name.
+   *
+   * @return The type name. This is meant for use as a type name in a struct schema. Other uses
+   *     should use {@link #getTypeString()}
+   */
+  public String getTypeName() {
+    return "Matrix__" + m_rows.getNum() + "_" + m_cols.getNum();
+  }
+
   @Override
   public String getTypeString() {
-    return "struct:Matrix__" + m_rows.getNum() + "_" + m_cols.getNum();
+    return "struct:" + getTypeName();
   }
 
   @Override

wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java

@@ -28,6 +28,7 @@
 import edu.wpi.first.math.numbers.N8;
 import edu.wpi.first.math.numbers.N9;
 import edu.wpi.first.math.system.proto.LinearSystemProto;
+import edu.wpi.first.math.system.struct.LinearSystemStruct;
 import edu.wpi.first.util.protobuf.Protobuf;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import java.util.Arrays;
@@ -383,4 +384,21 @@ LinearSystemProto<States, Inputs, Outputs> getProto(
           Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
     return new LinearSystemProto<>(states, inputs, outputs);
   }
+
+  /**
+   * Creates an implementation of the {@link Struct} interface for linear systems.
+   *
+   * @param <States> The number of states of the linear systems this serializer processes.
+   * @param <Inputs> The number of inputs of the linear systems this serializer processes.
+   * @param <Outputs> The number of outputs of the linear systems this serializer processes.
+   * @param states The number of states of the linear systems this serializer processes.
+   * @param inputs The number of inputs of the linear systems this serializer processes.
+   * @param outputs The number of outputs of the linear systems this serializer processes.
+   * @return The struct implementation.
+   */
+  public static <States extends Num, Inputs extends Num, Outputs extends Num>
+      LinearSystemStruct<States, Inputs, Outputs> getStruct(
+          Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
+    return new LinearSystemStruct<>(states, inputs, outputs);
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/system/struct/LinearSystemStruct.java

@@ -0,0 +1,94 @@
+// 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.system.struct;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.struct.MatrixStruct;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public final class LinearSystemStruct<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements Struct<LinearSystem<States, Inputs, Outputs>> {
+  private final Nat<States> m_states;
+  private final Nat<Inputs> m_inputs;
+  private final Nat<Outputs> m_outputs;
+  private final MatrixStruct<States, States> m_AStruct;
+  private final MatrixStruct<States, Inputs> m_BStruct;
+  private final MatrixStruct<Outputs, States> m_CStruct;
+  private final MatrixStruct<Outputs, Inputs> m_DStruct;
+
+  /**
+   * Constructs the {@link Struct} implementation.
+   *
+   * @param states The number of states of the linear systems this serializer processes.
+   * @param inputs The number of inputs of the linear systems this serializer processes.
+   * @param outputs The number of outputs of the linear systems this serializer processes.
+   */
+  public LinearSystemStruct(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
+    m_states = states;
+    m_inputs = inputs;
+    m_outputs = outputs;
+    m_AStruct = Matrix.getStruct(states, states);
+    m_BStruct = Matrix.getStruct(states, inputs);
+    m_CStruct = Matrix.getStruct(outputs, states);
+    m_DStruct = Matrix.getStruct(outputs, inputs);
+  }
+
+  @Override
+  public Class<LinearSystem<States, Inputs, Outputs>> getTypeClass() {
+    @SuppressWarnings("unchecked")
+    var clazz = (Class<LinearSystem<States, Inputs, Outputs>>) (Class<?>) LinearSystem.class;
+    return clazz;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:LinearSystem__"
+        + m_states.getNum()
+        + "_"
+        + m_inputs.getNum()
+        + "_"
+        + m_outputs.getNum();
+  }
+
+  @Override
+  public int getSize() {
+    return m_AStruct.getSize() + m_BStruct.getSize() + m_CStruct.getSize() + m_DStruct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return m_AStruct.getTypeName()
+        + " a;"
+        + m_BStruct.getTypeName()
+        + " b;"
+        + m_CStruct.getTypeName()
+        + " c;"
+        + m_DStruct.getTypeName()
+        + " d";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {m_AStruct, m_BStruct, m_CStruct, m_DStruct};
+  }
+
+  @Override
+  public LinearSystem<States, Inputs, Outputs> unpack(ByteBuffer bb) {
+    return new LinearSystem<>(
+        m_AStruct.unpack(bb), m_BStruct.unpack(bb), m_CStruct.unpack(bb), m_DStruct.unpack(bb));
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, LinearSystem<States, Inputs, Outputs> value) {
+    m_AStruct.pack(bb, value.getA());
+    m_BStruct.pack(bb, value.getB());
+    m_CStruct.pack(bb, value.getC());
+    m_DStruct.pack(bb, value.getD());
+  }
+}

wpimath/src/main/native/include/frc/struct/MatrixStruct.h

@@ -13,9 +13,10 @@
 template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
   requires(Cols != 1)
 struct wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
-  static constexpr ct_string kTypeString =
-      wpi::Concat("struct:Matrix__"_ct_string, wpi::NumToCtString<Rows>(),
+  static constexpr ct_string kTypeName =
+      wpi::Concat("Matrix__"_ct_string, wpi::NumToCtString<Rows>(),
                   "_"_ct_string, wpi::NumToCtString<Cols>());
+  static constexpr ct_string kTypeString = "struct:"_ct_string + kTypeName;
   static constexpr std::string_view GetTypeString() { return kTypeString; }
   static constexpr size_t GetSize() { return Rows * Cols * 8; }
   static constexpr ct_string kSchema =

wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.h

@@ -0,0 +1,43 @@
+// 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 <fmt/format.h>
+#include <wpi/ct_string.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/struct/MatrixStruct.h"
+#include "frc/system/LinearSystem.h"
+
+template <int States, int Inputs, int Outputs>
+struct wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>> {
+  static constexpr ct_string kTypeString = wpi::Concat(
+      "struct:LinearSystem__"_ct_string, wpi::NumToCtString<States>(),
+      "_"_ct_string, wpi::NumToCtString<Inputs>(), "_"_ct_string,
+      wpi::NumToCtString<Outputs>());
+  static constexpr std::string_view GetTypeString() { return kTypeString; }
+  static constexpr size_t GetSize() {
+    return wpi::Struct<frc::Matrixd<States, States>>::GetSize() +
+           wpi::Struct<frc::Matrixd<States, Inputs>>::GetSize() +
+           wpi::Struct<frc::Matrixd<Outputs, States>>::GetSize() +
+           wpi::Struct<frc::Matrixd<Outputs, Inputs>>::GetSize();
+  }
+  static constexpr ct_string kSchema = wpi::Concat(
+      wpi::Struct<frc::Matrixd<States, States>>::kTypeName, " a;"_ct_string,
+      wpi::Struct<frc::Matrixd<States, Inputs>>::kTypeName, " b;"_ct_string,
+      wpi::Struct<frc::Matrixd<Outputs, States>>::kTypeName, " c;"_ct_string,
+      wpi::Struct<frc::Matrixd<Outputs, Inputs>>::kTypeName, " d"_ct_string);
+  static constexpr std::string_view GetSchema() { return kSchema; }
+
+  static frc::LinearSystem<States, Inputs, Outputs> Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::LinearSystem<States, Inputs, Outputs>& value);
+};
+
+static_assert(wpi::StructSerializable<frc::LinearSystem<4, 3, 2>>);
+static_assert(wpi::StructSerializable<frc::LinearSystem<2, 3, 4>>);
+
+#include "frc/system/struct/LinearSystemStruct.inc"

wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.inc

@@ -0,0 +1,42 @@
+// 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 "frc/system/struct/LinearSystemStruct.h"
+
+template <int States, int Inputs, int Outputs>
+frc::LinearSystem<States, Inputs, Outputs>
+wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
+    std::span<const uint8_t> data) {
+  constexpr size_t kAOff = 0;
+  constexpr size_t kBOff =
+      kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
+  constexpr size_t kCOff =
+      kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
+  constexpr size_t kDOff =
+      kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
+  return frc::LinearSystem<States, Inputs, Outputs>{
+      wpi::UnpackStruct<frc::Matrixd<States, States>, kAOff>(data),
+      wpi::UnpackStruct<frc::Matrixd<States, Inputs>, kBOff>(data),
+      wpi::UnpackStruct<frc::Matrixd<Outputs, States>, kCOff>(data),
+      wpi::UnpackStruct<frc::Matrixd<Outputs, Inputs>, kDOff>(data)};
+}
+
+template <int States, int Inputs, int Outputs>
+void wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Pack(
+    std::span<uint8_t> data,
+    const frc::LinearSystem<States, Inputs, Outputs>& value) {
+  constexpr size_t kAOff = 0;
+  constexpr size_t kBOff =
+      kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
+  constexpr size_t kCOff =
+      kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
+  constexpr size_t kDOff =
+      kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
+  wpi::PackStruct<kAOff>(data, value.A());
+  wpi::PackStruct<kBOff>(data, value.B());
+  wpi::PackStruct<kCOff>(data, value.C());
+  wpi::PackStruct<kDOff>(data, value.D());
+}

wpimath/src/test/java/edu/wpi/first/math/system/struct/LinearSystemStructTest.java

@@ -0,0 +1,38 @@
+// 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.system.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class LinearSystemStructTest extends StructTestBase<LinearSystem<N2, N3, N4>> {
+  LinearSystemStructTest() {
+    super(
+        new LinearSystem<>(
+            MatBuilder.fill(Nat.N2(), Nat.N2(), 1.1, 1.2, 1.3, 1.4),
+            MatBuilder.fill(Nat.N2(), Nat.N3(), 2.1, 2.2, 2.3, 2.4, 2.5, 2.6),
+            MatBuilder.fill(Nat.N4(), Nat.N2(), 3.1, 3.2, 3.3, 3.4, 3.5, 3.6, 3.7, 3.8),
+            MatBuilder.fill(
+                Nat.N4(), Nat.N3(), 4.01, 4.02, 4.03, 4.04, 4.05, 4.06, 4.07, 4.08, 4.09, 4.10,
+                4.11, 4.12)),
+        LinearSystem.getStruct(Nat.N2(), Nat.N3(), Nat.N4()));
+  }
+
+  @Override
+  public void checkEquals(LinearSystem<N2, N3, N4> testData, LinearSystem<N2, N3, N4> data) {
+    assertEquals(testData.getA(), data.getA());
+    assertEquals(testData.getB(), data.getB());
+    assertEquals(testData.getC(), data.getC());
+    assertEquals(testData.getD(), data.getD());
+  }
+}

wpimath/src/test/native/cpp/system/struct/LinearSystemStructTest.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 <gtest/gtest.h>
+
+#include "../../StructTestBase.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/struct/LinearSystemStruct.h"
+
+using namespace frc;
+
+struct LinearSystemStructTestData {
+  using Type = LinearSystem<2, 3, 4>;
+
+  inline static const Type kTestData{
+      Matrixd<2, 2>{{1.1, 1.2}, {1.3, 1.4}},
+      Matrixd<2, 3>{{2.1, 2.2, 2.3}, {2.4, 2.5, 2.6}},
+      Matrixd<4, 2>{{3.1, 3.2}, {3.3, 3.4}, {3.5, 3.6}, {3.7, 3.8}},
+      Matrixd<4, 3>{{4.01, 4.02, 4.03},
+                    {4.04, 4.05, 4.06},
+                    {4.07, 4.08, 4.09},
+                    {4.10, 4.11, 4.12}}};
+
+  static void CheckEq(const Type& testData, const Type& data) {
+    EXPECT_EQ(testData.A(), data.A());
+    EXPECT_EQ(testData.B(), data.B());
+    EXPECT_EQ(testData.C(), data.C());
+    EXPECT_EQ(testData.D(), data.D());
+  }
+};
+
+INSTANTIATE_TYPED_TEST_SUITE_P(LinearSystem, StructTest,
+                               LinearSystemStructTestData);