[wpimath] Add remaining struct and protobuf implementations (#5953)

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

@@ -5,6 +5,7 @@
 package edu.wpi.first.math.system;
 
 import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N10;
@@ -26,6 +27,12 @@ import edu.wpi.first.math.numbers.N6;
 import edu.wpi.first.math.numbers.N7;
 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 edu.wpi.first.util.struct.Struct;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Arrays;
 import java.util.Collections;
 import java.util.List;
@@ -44,7 +51,8 @@ import org.ejml.simple.SimpleMatrix;
  * @param <Inputs> Number of inputs.
  * @param <Outputs> Number of outputs.
  */
-public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
+public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements ProtobufSerializable, StructSerializable {
   /** Continuous system matrix. */
   private final Matrix<States, States> m_A;
 
@@ -361,4 +369,38 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
         "Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n",
         m_A.toString(), m_B.toString(), m_C.toString(), m_D.toString());
   }
+
+  /**
+   * Creates an implementation of the {@link Protobuf} 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 protobuf implementation.
+   */
+  public static <States extends Num, Inputs extends Num, Outputs extends Num>
+      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/proto/LinearSystemProto.java

@@ -0,0 +1,99 @@
+// 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.proto;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.proto.System.ProtobufLinearSystem;
+import edu.wpi.first.math.proto.Wpimath.ProtobufMatrix;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public final class LinearSystemProto<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements Protobuf<LinearSystem<States, Inputs, Outputs>, ProtobufLinearSystem> {
+  private final Nat<States> m_states;
+  private final Nat<Inputs> m_inputs;
+  private final Nat<Outputs> m_outputs;
+  private final Protobuf<Matrix<States, States>, ProtobufMatrix> m_AProto;
+  private final Protobuf<Matrix<States, Inputs>, ProtobufMatrix> m_BProto;
+  private final Protobuf<Matrix<Outputs, States>, ProtobufMatrix> m_CProto;
+  private final Protobuf<Matrix<Outputs, Inputs>, ProtobufMatrix> m_DProto;
+
+  /**
+   * Constructs the {@link Protobuf} 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 LinearSystemProto(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
+    m_states = states;
+    m_inputs = inputs;
+    m_outputs = outputs;
+    m_AProto = Matrix.getProto(states, states);
+    m_BProto = Matrix.getProto(states, inputs);
+    m_CProto = Matrix.getProto(outputs, states);
+    m_DProto = Matrix.getProto(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 Descriptor getDescriptor() {
+    return ProtobufLinearSystem.getDescriptor();
+  }
+
+  @Override
+  public ProtobufLinearSystem createMessage() {
+    return ProtobufLinearSystem.newInstance();
+  }
+
+  @Override
+  public LinearSystem<States, Inputs, Outputs> unpack(ProtobufLinearSystem msg) {
+    if (msg.getNumStates() != m_states.getNum()
+        || msg.getNumInputs() != m_inputs.getNum()
+        || msg.getNumOutputs() != m_outputs.getNum()) {
+      throw new IllegalArgumentException(
+          "Tried to unpack msg "
+              + msg
+              + " with "
+              + msg.getNumStates()
+              + " states and "
+              + msg.getNumInputs()
+              + " inputs and "
+              + msg.getNumOutputs()
+              + " outputs into LinearSystem with "
+              + m_states.getNum()
+              + " states "
+              + m_inputs.getNum()
+              + " inputs "
+              + m_outputs.getNum()
+              + " outputs");
+    }
+    return new LinearSystem<>(
+        m_AProto.unpack(msg.getA()),
+        m_BProto.unpack(msg.getB()),
+        m_CProto.unpack(msg.getC()),
+        m_DProto.unpack(msg.getD()));
+  }
+
+  @Override
+  public void pack(ProtobufLinearSystem msg, LinearSystem<States, Inputs, Outputs> value) {
+    msg.setNumStates(m_states.getNum())
+        .setNumInputs(m_inputs.getNum())
+        .setNumOutputs(m_outputs.getNum());
+    m_AProto.pack(msg.getMutableA(), value.getA());
+    m_BProto.pack(msg.getMutableB(), value.getB());
+    m_CProto.pack(msg.getMutableC(), value.getC());
+    m_DProto.pack(msg.getMutableD(), value.getD());
+  }
+}

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

@@ -0,0 +1,89 @@
+// 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 int m_states;
+  private final int m_inputs;
+  private final int 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.getNum();
+    m_inputs = inputs.getNum();
+    m_outputs = outputs.getNum();
+    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 getTypeName() {
+    return "LinearSystem__" + m_states + "_" + m_inputs + "_" + m_outputs;
+  }
+
+  @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());
+  }
+}