From c8dfb430240e4e11e52c975eef1404ed82440621 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 12 Jul 2024 19:40:11 -0700 Subject: [PATCH] Add struct implementations for LinearSystem --- .../wpi/first/math/struct/MatrixStruct.java | 12 ++- .../wpi/first/math/system/LinearSystem.java | 18 ++++ .../system/struct/LinearSystemStruct.java | 94 +++++++++++++++++++ .../native/include/frc/struct/MatrixStruct.h | 5 +- .../frc/system/struct/LinearSystemStruct.h | 43 +++++++++ .../frc/system/struct/LinearSystemStruct.inc | 42 +++++++++ .../system/struct/LinearSystemStructTest.java | 38 ++++++++ .../system/struct/LinearSystemStructTest.cpp | 34 +++++++ 8 files changed, 283 insertions(+), 3 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/system/struct/LinearSystemStruct.java create mode 100644 wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.h create mode 100644 wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.inc create mode 100644 wpimath/src/test/java/edu/wpi/first/math/system/struct/LinearSystemStructTest.java create mode 100644 wpimath/src/test/native/cpp/system/struct/LinearSystemStructTest.cpp diff --git a/wpimath/src/main/java/edu/wpi/first/math/struct/MatrixStruct.java b/wpimath/src/main/java/edu/wpi/first/math/struct/MatrixStruct.java index c0805c1386b..b55f8767f6e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/struct/MatrixStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/struct/MatrixStruct.java @@ -33,9 +33,19 @@ public Class> 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 diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java index 5da8e69c49a..0780d070b8f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java +++ b/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 getProto( Nat states, Nat inputs, Nat outputs) { return new LinearSystemProto<>(states, inputs, outputs); } + + /** + * Creates an implementation of the {@link Struct} interface for linear systems. + * + * @param The number of states of the linear systems this serializer processes. + * @param The number of inputs of the linear systems this serializer processes. + * @param 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 + LinearSystemStruct getStruct( + Nat states, Nat inputs, Nat outputs) { + return new LinearSystemStruct<>(states, inputs, outputs); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/struct/LinearSystemStruct.java b/wpimath/src/main/java/edu/wpi/first/math/system/struct/LinearSystemStruct.java new file mode 100644 index 00000000000..2669bb81686 --- /dev/null +++ b/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 + implements Struct> { + private final Nat m_states; + private final Nat m_inputs; + private final Nat m_outputs; + private final MatrixStruct m_AStruct; + private final MatrixStruct m_BStruct; + private final MatrixStruct m_CStruct; + private final MatrixStruct 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, Nat inputs, Nat 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> getTypeClass() { + @SuppressWarnings("unchecked") + var clazz = (Class>) (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 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 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()); + } +} diff --git a/wpimath/src/main/native/include/frc/struct/MatrixStruct.h b/wpimath/src/main/native/include/frc/struct/MatrixStruct.h index cd655e1cfb3..92fe8b55917 100644 --- a/wpimath/src/main/native/include/frc/struct/MatrixStruct.h +++ b/wpimath/src/main/native/include/frc/struct/MatrixStruct.h @@ -13,9 +13,10 @@ template requires(Cols != 1) struct wpi::Struct> { - static constexpr ct_string kTypeString = - wpi::Concat("struct:Matrix__"_ct_string, wpi::NumToCtString(), + static constexpr ct_string kTypeName = + wpi::Concat("Matrix__"_ct_string, wpi::NumToCtString(), "_"_ct_string, wpi::NumToCtString()); + 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 = diff --git a/wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.h b/wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.h new file mode 100644 index 00000000000..21272c2ba2f --- /dev/null +++ b/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 +#include +#include + +#include "frc/struct/MatrixStruct.h" +#include "frc/system/LinearSystem.h" + +template +struct wpi::Struct> { + static constexpr ct_string kTypeString = wpi::Concat( + "struct:LinearSystem__"_ct_string, wpi::NumToCtString(), + "_"_ct_string, wpi::NumToCtString(), "_"_ct_string, + wpi::NumToCtString()); + static constexpr std::string_view GetTypeString() { return kTypeString; } + static constexpr size_t GetSize() { + return wpi::Struct>::GetSize() + + wpi::Struct>::GetSize() + + wpi::Struct>::GetSize() + + wpi::Struct>::GetSize(); + } + static constexpr ct_string kSchema = wpi::Concat( + wpi::Struct>::kTypeName, " a;"_ct_string, + wpi::Struct>::kTypeName, " b;"_ct_string, + wpi::Struct>::kTypeName, " c;"_ct_string, + wpi::Struct>::kTypeName, " d"_ct_string); + static constexpr std::string_view GetSchema() { return kSchema; } + + static frc::LinearSystem Unpack( + std::span data); + static void Pack(std::span data, + const frc::LinearSystem& value); +}; + +static_assert(wpi::StructSerializable>); +static_assert(wpi::StructSerializable>); + +#include "frc/system/struct/LinearSystemStruct.inc" diff --git a/wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.inc b/wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.inc new file mode 100644 index 00000000000..fbac4b97bcd --- /dev/null +++ b/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 +frc::LinearSystem +wpi::Struct>::Unpack( + std::span data) { + constexpr size_t kAOff = 0; + constexpr size_t kBOff = + kAOff + wpi::GetStructSize>(); + constexpr size_t kCOff = + kBOff + wpi::GetStructSize>(); + constexpr size_t kDOff = + kCOff + wpi::GetStructSize>(); + return frc::LinearSystem{ + wpi::UnpackStruct, kAOff>(data), + wpi::UnpackStruct, kBOff>(data), + wpi::UnpackStruct, kCOff>(data), + wpi::UnpackStruct, kDOff>(data)}; +} + +template +void wpi::Struct>::Pack( + std::span data, + const frc::LinearSystem& value) { + constexpr size_t kAOff = 0; + constexpr size_t kBOff = + kAOff + wpi::GetStructSize>(); + constexpr size_t kCOff = + kBOff + wpi::GetStructSize>(); + constexpr size_t kDOff = + kCOff + wpi::GetStructSize>(); + wpi::PackStruct(data, value.A()); + wpi::PackStruct(data, value.B()); + wpi::PackStruct(data, value.C()); + wpi::PackStruct(data, value.D()); +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/struct/LinearSystemStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/struct/LinearSystemStructTest.java new file mode 100644 index 00000000000..4dc56dabc9c --- /dev/null +++ b/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> { + 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 testData, LinearSystem data) { + assertEquals(testData.getA(), data.getA()); + assertEquals(testData.getB(), data.getB()); + assertEquals(testData.getC(), data.getC()); + assertEquals(testData.getD(), data.getD()); + } +} diff --git a/wpimath/src/test/native/cpp/system/struct/LinearSystemStructTest.cpp b/wpimath/src/test/native/cpp/system/struct/LinearSystemStructTest.cpp new file mode 100644 index 00000000000..b1d01b01eca --- /dev/null +++ b/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 + +#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);