diff --git a/docs/build.gradle b/docs/build.gradle index 8225c2fbfe..5ac8c83d0f 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -221,12 +221,17 @@ task generateJavaDocs(type: Javadoc) { "-edu.wpi.first.hal.simulation," + // TODO: ^ Document these, then remove them from the list "-edu.wpi.first.math.proto," + + "-edu.wpi.first.math.struct," + "-edu.wpi.first.math.controller.proto," + "-edu.wpi.first.math.controller.struct," + "-edu.wpi.first.math.geometry.proto," + "-edu.wpi.first.math.geometry.struct," + "-edu.wpi.first.math.kinematics.proto," + "-edu.wpi.first.math.kinematics.struct," + + "-edu.wpi.first.math.spline.proto," + + "-edu.wpi.first.math.spline.struct," + + "-edu.wpi.first.math.system.proto," + + "-edu.wpi.first.math.system.struct," + "-edu.wpi.first.math.system.plant.proto," + "-edu.wpi.first.math.system.plant.struct," + "-edu.wpi.first.math.trajectory.proto", true) diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index f63490a277..e017fc9a1b 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -139,6 +139,10 @@ target_compile_definitions(wpimath PRIVATE WPILIB_EXPORTS SLEIPNIR_EXPORTS) target_compile_features(wpimath PUBLIC cxx_std_20) if(MSVC) target_compile_options(wpimath PUBLIC /utf-8 /bigobj) + target_link_options( + wpimath + PRIVATE /DEF:$ + ) endif() wpilib_target_warnings(wpimath) target_link_libraries(wpimath wpiutil) diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java index ed423da583..1498b67a62 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java @@ -18,36 +18,37 @@ import us.hebi.quickbuf.ProtoUtil; import us.hebi.quickbuf.RepeatedByte; public final class Controller { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1684, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1755, "ChBjb250cm9sbGVyLnByb3RvEgl3cGkucHJvdG8iWAoWUHJvdG9idWZBcm1GZWVkZm9yd2FyZBIOCgJr" + "cxgBIAEoAVICa3MSDgoCa2cYAiABKAFSAmtnEg4KAmt2GAMgASgBUgJrdhIOCgJrYRgEIAEoAVICa2Ei" + "ngEKJFByb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVGZWVkZm9yd2FyZBIbCglrdl9saW5lYXIYASABKAFS" + "CGt2TGluZWFyEhsKCWthX2xpbmVhchgCIAEoAVIIa2FMaW5lYXISHQoKa3ZfYW5ndWxhchgDIAEoAVIJ" + "a3ZBbmd1bGFyEh0KCmthX2FuZ3VsYXIYBCABKAFSCWthQW5ndWxhciJdChtQcm90b2J1ZkVsZXZhdG9y" + "RmVlZGZvcndhcmQSDgoCa3MYASABKAFSAmtzEg4KAmtnGAIgASgBUgJrZxIOCgJrdhgDIAEoAVICa3YS" + - "DgoCa2EYBCABKAFSAmthIlAKHlByb3RvYnVmU2ltcGxlTW90b3JGZWVkZm9yd2FyZBIOCgJrcxgBIAEo" + - "AVICa3MSDgoCa3YYAiABKAFSAmt2Eg4KAmthGAMgASgBUgJrYSJSCiZQcm90b2J1ZkRpZmZlcmVudGlh" + - "bERyaXZlV2hlZWxWb2x0YWdlcxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdo" + - "dEIaChhlZHUud3BpLmZpcnN0Lm1hdGgucHJvdG9K0AgKBhIEAAAkAQoICgEMEgMAABIKCAoBAhIDAgAS" + - "CggKAQgSAwQAMQoJCgIIARIDBAAxCgoKAgQAEgQGAAsBCgoKAwQAARIDBggeCgsKBAQAAgASAwcCEAoM" + - "CgUEAAIABRIDBwIICgwKBQQAAgABEgMHCQsKDAoFBAACAAMSAwcODwoLCgQEAAIBEgMIAhAKDAoFBAAC" + - "AQUSAwgCCAoMCgUEAAIBARIDCAkLCgwKBQQAAgEDEgMIDg8KCwoEBAACAhIDCQIQCgwKBQQAAgIFEgMJ" + - "AggKDAoFBAACAgESAwkJCwoMCgUEAAICAxIDCQ4PCgsKBAQAAgMSAwoCEAoMCgUEAAIDBRIDCgIICgwK" + - "BQQAAgMBEgMKCQsKDAoFBAACAwMSAwoODwoKCgIEARIEDQASAQoKCgMEAQESAw0ILAoLCgQEAQIAEgMO" + - "AhcKDAoFBAECAAUSAw4CCAoMCgUEAQIAARIDDgkSCgwKBQQBAgADEgMOFRYKCwoEBAECARIDDwIXCgwK" + - "BQQBAgEFEgMPAggKDAoFBAECAQESAw8JEgoMCgUEAQIBAxIDDxUWCgsKBAQBAgISAxACGAoMCgUEAQIC" + - "BRIDEAIICgwKBQQBAgIBEgMQCRMKDAoFBAECAgMSAxAWFwoLCgQEAQIDEgMRAhgKDAoFBAECAwUSAxEC" + - "CAoMCgUEAQIDARIDEQkTCgwKBQQBAgMDEgMRFhcKCgoCBAISBBQAGQEKCgoDBAIBEgMUCCMKCwoEBAIC" + - "ABIDFQIQCgwKBQQCAgAFEgMVAggKDAoFBAICAAESAxUJCwoMCgUEAgIAAxIDFQ4PCgsKBAQCAgESAxYC", - "EAoMCgUEAgIBBRIDFgIICgwKBQQCAgEBEgMWCQsKDAoFBAICAQMSAxYODwoLCgQEAgICEgMXAhAKDAoF" + - "BAICAgUSAxcCCAoMCgUEAgICARIDFwkLCgwKBQQCAgIDEgMXDg8KCwoEBAICAxIDGAIQCgwKBQQCAgMF" + - "EgMYAggKDAoFBAICAwESAxgJCwoMCgUEAgIDAxIDGA4PCgoKAgQDEgQbAB8BCgoKAwQDARIDGwgmCgsK" + - "BAQDAgASAxwCEAoMCgUEAwIABRIDHAIICgwKBQQDAgABEgMcCQsKDAoFBAMCAAMSAxwODwoLCgQEAwIB" + - "EgMdAhAKDAoFBAMCAQUSAx0CCAoMCgUEAwIBARIDHQkLCgwKBQQDAgEDEgMdDg8KCwoEBAMCAhIDHgIQ" + - "CgwKBQQDAgIFEgMeAggKDAoFBAMCAgESAx4JCwoMCgUEAwICAxIDHg4PCgoKAgQEEgQhACQBCgoKAwQE" + - "ARIDIQguCgsKBAQEAgASAyICEgoMCgUEBAIABRIDIgIICgwKBQQEAgABEgMiCQ0KDAoFBAQCAAMSAyIQ" + - "EQoLCgQEBAIBEgMjAhMKDAoFBAQCAQUSAyMCCAoMCgUEBAIBARIDIwkOCgwKBQQEAgEDEgMjERJiBnBy" + - "b3RvMw=="); + "DgoCa2EYBCABKAFSAmthImAKHlByb3RvYnVmU2ltcGxlTW90b3JGZWVkZm9yd2FyZBIOCgJrcxgBIAEo" + + "AVICa3MSDgoCa3YYAiABKAFSAmt2Eg4KAmthGAMgASgBUgJrYRIOCgJkdBgEIAEoAVICZHQiUgomUHJv" + + "dG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsVm9sdGFnZXMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVy" + + "aWdodBgCIAEoAVIFcmlnaHRCGgoYZWR1LndwaS5maXJzdC5tYXRoLnByb3RvSocJCgYSBAAAJQEKCAoB" + + "DBIDAAASCggKAQISAwIAEgoICgEIEgMEADEKCQoCCAESAwQAMQoKCgIEABIEBgALAQoKCgMEAAESAwYI" + + "HgoLCgQEAAIAEgMHAhAKDAoFBAACAAUSAwcCCAoMCgUEAAIAARIDBwkLCgwKBQQAAgADEgMHDg8KCwoE" + + "BAACARIDCAIQCgwKBQQAAgEFEgMIAggKDAoFBAACAQESAwgJCwoMCgUEAAIBAxIDCA4PCgsKBAQAAgIS" + + "AwkCEAoMCgUEAAICBRIDCQIICgwKBQQAAgIBEgMJCQsKDAoFBAACAgMSAwkODwoLCgQEAAIDEgMKAhAK" + + "DAoFBAACAwUSAwoCCAoMCgUEAAIDARIDCgkLCgwKBQQAAgMDEgMKDg8KCgoCBAESBA0AEgEKCgoDBAEB" + + "EgMNCCwKCwoEBAECABIDDgIXCgwKBQQBAgAFEgMOAggKDAoFBAECAAESAw4JEgoMCgUEAQIAAxIDDhUW" + + "CgsKBAQBAgESAw8CFwoMCgUEAQIBBRIDDwIICgwKBQQBAgEBEgMPCRIKDAoFBAECAQMSAw8VFgoLCgQE" + + "AQICEgMQAhgKDAoFBAECAgUSAxACCAoMCgUEAQICARIDEAkTCgwKBQQBAgIDEgMQFhcKCwoEBAECAxID" + + "EQIYCgwKBQQBAgMFEgMRAggKDAoFBAECAwESAxEJEwoMCgUEAQIDAxIDERYXCgoKAgQCEgQUABkBCgoK" + + "AwQCARIDFAgjCgsKBAQCAgASAxUCEAoMCgUEAgIABRIDFQIICgwKBQQCAgABEgMVCQsKDAoFBAICAAMS", + "AxUODwoLCgQEAgIBEgMWAhAKDAoFBAICAQUSAxYCCAoMCgUEAgIBARIDFgkLCgwKBQQCAgEDEgMWDg8K" + + "CwoEBAICAhIDFwIQCgwKBQQCAgIFEgMXAggKDAoFBAICAgESAxcJCwoMCgUEAgICAxIDFw4PCgsKBAQC" + + "AgMSAxgCEAoMCgUEAgIDBRIDGAIICgwKBQQCAgMBEgMYCQsKDAoFBAICAwMSAxgODwoKCgIEAxIEGwAg" + + "AQoKCgMEAwESAxsIJgoLCgQEAwIAEgMcAhAKDAoFBAMCAAUSAxwCCAoMCgUEAwIAARIDHAkLCgwKBQQD" + + "AgADEgMcDg8KCwoEBAMCARIDHQIQCgwKBQQDAgEFEgMdAggKDAoFBAMCAQESAx0JCwoMCgUEAwIBAxID" + + "HQ4PCgsKBAQDAgISAx4CEAoMCgUEAwICBRIDHgIICgwKBQQDAgIBEgMeCQsKDAoFBAMCAgMSAx4ODwoL" + + "CgQEAwIDEgMfAhAKDAoFBAMCAwUSAx8CCAoMCgUEAwIDARIDHwkLCgwKBQQDAgMDEgMfDg8KCgoCBAQS" + + "BCIAJQEKCgoDBAQBEgMiCC4KCwoEBAQCABIDIwISCgwKBQQEAgAFEgMjAggKDAoFBAQCAAESAyMJDQoM" + + "CgUEBAIAAxIDIxARCgsKBAQEAgESAyQCEwoMCgUEBAIBBRIDJAIICgwKBQQEAgEBEgMkCQ4KDAoFBAQC" + + "AQMSAyQREmIGcHJvdG8z"); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("controller.proto", "wpi.proto", descriptorData); @@ -57,9 +58,9 @@ public final class Controller { static final Descriptors.Descriptor wpi_proto_ProtobufElevatorFeedforward_descriptor = descriptor.internalContainedType(282, 93, "ProtobufElevatorFeedforward", "wpi.proto.ProtobufElevatorFeedforward"); - static final Descriptors.Descriptor wpi_proto_ProtobufSimpleMotorFeedforward_descriptor = descriptor.internalContainedType(377, 80, "ProtobufSimpleMotorFeedforward", "wpi.proto.ProtobufSimpleMotorFeedforward"); + static final Descriptors.Descriptor wpi_proto_ProtobufSimpleMotorFeedforward_descriptor = descriptor.internalContainedType(377, 96, "ProtobufSimpleMotorFeedforward", "wpi.proto.ProtobufSimpleMotorFeedforward"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelVoltages_descriptor = descriptor.internalContainedType(459, 82, "ProtobufDifferentialDriveWheelVoltages", "wpi.proto.ProtobufDifferentialDriveWheelVoltages"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelVoltages_descriptor = descriptor.internalContainedType(475, 82, "ProtobufDifferentialDriveWheelVoltages", "wpi.proto.ProtobufDifferentialDriveWheelVoltages"); /** * @return this proto file's descriptor. @@ -1576,6 +1577,11 @@ public final class Controller { */ private double ka; + /** + * optional double dt = 4; + */ + private double dt; + private ProtobufSimpleMotorFeedforward() { } @@ -1697,6 +1703,43 @@ public final class Controller { return this; } + /** + * optional double dt = 4; + * @return whether the dt field is set + */ + public boolean hasDt() { + return (bitField0_ & 0x00000008) != 0; + } + + /** + * optional double dt = 4; + * @return this + */ + public ProtobufSimpleMotorFeedforward clearDt() { + bitField0_ &= ~0x00000008; + dt = 0D; + return this; + } + + /** + * optional double dt = 4; + * @return the dt + */ + public double getDt() { + return dt; + } + + /** + * optional double dt = 4; + * @param value the dt to set + * @return this + */ + public ProtobufSimpleMotorFeedforward setDt(final double value) { + bitField0_ |= 0x00000008; + dt = value; + return this; + } + @Override public ProtobufSimpleMotorFeedforward copyFrom(final ProtobufSimpleMotorFeedforward other) { cachedSize = other.cachedSize; @@ -1705,6 +1748,7 @@ public final class Controller { ks = other.ks; kv = other.kv; ka = other.ka; + dt = other.dt; } return this; } @@ -1724,6 +1768,9 @@ public final class Controller { if (other.hasKa()) { setKa(other.ka); } + if (other.hasDt()) { + setDt(other.dt); + } return this; } @@ -1737,6 +1784,7 @@ public final class Controller { ks = 0D; kv = 0D; ka = 0D; + dt = 0D; return this; } @@ -1762,7 +1810,8 @@ public final class Controller { return bitField0_ == other.bitField0_ && (!hasKs() || ProtoUtil.isEqual(ks, other.ks)) && (!hasKv() || ProtoUtil.isEqual(kv, other.kv)) - && (!hasKa() || ProtoUtil.isEqual(ka, other.ka)); + && (!hasKa() || ProtoUtil.isEqual(ka, other.ka)) + && (!hasDt() || ProtoUtil.isEqual(dt, other.dt)); } @Override @@ -1779,6 +1828,10 @@ public final class Controller { output.writeRawByte((byte) 25); output.writeDoubleNoTag(ka); } + if ((bitField0_ & 0x00000008) != 0) { + output.writeRawByte((byte) 33); + output.writeDoubleNoTag(dt); + } } @Override @@ -1793,6 +1846,9 @@ public final class Controller { if ((bitField0_ & 0x00000004) != 0) { size += 9; } + if ((bitField0_ & 0x00000008) != 0) { + size += 9; + } return size; } @@ -1826,6 +1882,15 @@ public final class Controller { ka = input.readDouble(); bitField0_ |= 0x00000004; tag = input.readTag(); + if (tag != 33) { + break; + } + } + case 33: { + // dt + dt = input.readDouble(); + bitField0_ |= 0x00000008; + tag = input.readTag(); if (tag != 0) { break; } @@ -1856,6 +1921,9 @@ public final class Controller { if ((bitField0_ & 0x00000004) != 0) { output.writeDouble(FieldNames.ka, ka); } + if ((bitField0_ & 0x00000008) != 0) { + output.writeDouble(FieldNames.dt, dt); + } output.endObject(); } @@ -1899,6 +1967,17 @@ public final class Controller { } break; } + case 3216: { + if (input.isAtField(FieldNames.dt)) { + if (!input.trySkipNullValue()) { + dt = input.readDouble(); + bitField0_ |= 0x00000008; + } + } else { + input.skipUnknownField(); + } + break; + } default: { input.skipUnknownField(); break; @@ -1966,6 +2045,8 @@ public final class Controller { static final FieldName kv = FieldName.forField("kv"); static final FieldName ka = FieldName.forField("ka"); + + static final FieldName dt = FieldName.forField("dt"); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java index ae5741ea13..250603bfcf 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java +++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java @@ -6,6 +6,12 @@ package edu.wpi.first.math; import edu.wpi.first.math.jni.EigenJNI; import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.proto.MatrixProto; +import edu.wpi.first.math.struct.MatrixStruct; +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.Objects; import org.ejml.MatrixDimensionException; import org.ejml.data.DMatrixRMaj; @@ -24,7 +30,8 @@ import org.ejml.simple.SimpleMatrix; * @param The number of rows in this matrix. * @param The number of columns in this matrix. */ -public class Matrix { +public class Matrix + implements ProtobufSerializable, StructSerializable { /** Storage for underlying EJML matrix. */ protected final SimpleMatrix m_storage; @@ -738,4 +745,32 @@ public class Matrix { public int hashCode() { return Objects.hash(m_storage); } + + /** + * Creates an implementation of the {@link Protobuf} interface for matrices. + * + * @param The number of rows of the matrices this serializer processes. + * @param The number of cols of the matrices this serializer processes. + * @param rows The number of rows of the matrices this serializer processes. + * @param cols The number of cols of the matrices this serializer processes. + * @return The protobuf implementation. + */ + public static MatrixProto getProto( + Nat rows, Nat cols) { + return new MatrixProto<>(rows, cols); + } + + /** + * Creates an implementation of the {@link Struct} interfaces for matrices. + * + * @param The number of rows of the matrices this serializer processes. + * @param The number of cols of the matrices this serializer processes. + * @param rows The number of rows of the matrices this serializer processes. + * @param cols The number of cols of the matrices this serializer processes. + * @return The struct implementation. + */ + public static MatrixStruct getStruct( + Nat rows, Nat cols) { + return new MatrixStruct<>(rows, cols); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/math/Vector.java index 8784a6c027..69e019dc8a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/Vector.java +++ b/wpimath/src/main/java/edu/wpi/first/math/Vector.java @@ -6,6 +6,12 @@ package edu.wpi.first.math; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.proto.VectorProto; +import edu.wpi.first.math.struct.VectorStruct; +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.Objects; import org.ejml.simple.SimpleMatrix; @@ -16,7 +22,8 @@ import org.ejml.simple.SimpleMatrix; * * @param The number of rows in this matrix. */ -public class Vector extends Matrix { +public class Vector extends Matrix + implements ProtobufSerializable, StructSerializable { /** * Constructs an empty zero vector of the given dimensions. * @@ -151,4 +158,26 @@ public class Vector extends Matrix { a.get(2) * b.get(0) - a.get(0) * b.get(2), a.get(0) * b.get(1) - a.get(1) * b.get(0)); } + + /** + * Creates an implementation of the {@link Protobuf} interface for vectors. + * + * @param The number of rows of the vectors this serializer processes. + * @param rows The number of rows of the vectors this serializer processes. + * @return The protobuf implementation. + */ + public static final VectorProto getProto(Nat rows) { + return new VectorProto<>(rows); + } + + /** + * Creates an implementation of the {@link Struct} interface for vectors. + * + * @param The number of rows of the vectors this serializer processes. + * @param rows The number of rows of the vectors this serializer processes. + * @return The struct implementation. + */ + public static final VectorStruct getStruct(Nat rows) { + return new VectorStruct<>(rows); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java index 4d0c94f0be..15be9f62f9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java @@ -5,14 +5,30 @@ package edu.wpi.first.math.controller; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.proto.DifferentialDriveFeedforwardProto; +import edu.wpi.first.math.controller.struct.DifferentialDriveFeedforwardStruct; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.StructSerializable; /** A helper class which computes the feedforward outputs for a differential drive drivetrain. */ -public class DifferentialDriveFeedforward { +public class DifferentialDriveFeedforward implements ProtobufSerializable, StructSerializable { private final LinearSystem m_plant; + /** The linear velocity gain in volts per (meters per second). */ + public final double m_kVLinear; + + /** The linear acceleration gain in volts per (meters per second squared). */ + public final double m_kALinear; + + /** The angular velocity gain in volts per (radians per second). */ + public final double m_kVAngular; + + /** The angular acceleration gain in volts per (radians per second squared). */ + public final double m_kAAngular; + /** * Creates a new DifferentialDriveFeedforward with the specified parameters. * @@ -25,9 +41,8 @@ public class DifferentialDriveFeedforward { */ public DifferentialDriveFeedforward( double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) { - m_plant = - LinearSystemId.identifyDrivetrainSystem( - kVLinear, kALinear, kVAngular, kAAngular, trackwidth); + // See LinearSystemId.identifyDrivetrainSystem(double, double, double, double, double) + this(kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth); } /** @@ -41,6 +56,10 @@ public class DifferentialDriveFeedforward { public DifferentialDriveFeedforward( double kVLinear, double kALinear, double kVAngular, double kAAngular) { m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular); + m_kVLinear = kVLinear; + m_kALinear = kALinear; + m_kVAngular = kVAngular; + m_kAAngular = kAAngular; } /** @@ -67,4 +86,12 @@ public class DifferentialDriveFeedforward { var u = feedforward.calculate(r, nextR); return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0)); } + + /** DifferentialDriveFeedforward struct for serialization. */ + public static final DifferentialDriveFeedforwardStruct struct = + new DifferentialDriveFeedforwardStruct(); + + /** DifferentialDriveFeedforward protobuf for serialization. */ + public static final DifferentialDriveFeedforwardProto proto = + new DifferentialDriveFeedforwardProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index f9f86073ee..771d927cb4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -6,13 +6,17 @@ package edu.wpi.first.math.controller; import static edu.wpi.first.units.Units.Volts; +import edu.wpi.first.math.controller.proto.SimpleMotorFeedforwardProto; +import edu.wpi.first.math.controller.struct.SimpleMotorFeedforwardStruct; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Unit; import edu.wpi.first.units.Velocity; import edu.wpi.first.units.Voltage; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.StructSerializable; /** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */ -public class SimpleMotorFeedforward { +public class SimpleMotorFeedforward implements ProtobufSerializable, StructSerializable { /** The static gain. */ private final double ks; @@ -287,4 +291,10 @@ public class SimpleMotorFeedforward { public double minAchievableAcceleration(double maxVoltage, double velocity) { return maxAchievableAcceleration(-maxVoltage, velocity); } + + /** SimpleMotorFeedforward struct for serialization. */ + public static final SimpleMotorFeedforwardStruct struct = new SimpleMotorFeedforwardStruct(); + + /** SimpleMotorFeedforward protobuf for serialization. */ + public static final SimpleMotorFeedforwardProto proto = new SimpleMotorFeedforwardProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProto.java new file mode 100644 index 0000000000..6daee8b3fd --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProto.java @@ -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. + +package edu.wpi.first.math.controller.proto; + +import edu.wpi.first.math.controller.DifferentialDriveFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveFeedforward; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public final class DifferentialDriveFeedforwardProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveFeedforward.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveFeedforward.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveFeedforward createMessage() { + return ProtobufDifferentialDriveFeedforward.newInstance(); + } + + @Override + public DifferentialDriveFeedforward unpack(ProtobufDifferentialDriveFeedforward msg) { + return new DifferentialDriveFeedforward( + msg.getKvLinear(), msg.getKaLinear(), msg.getKvAngular(), msg.getKaAngular()); + } + + @Override + public void pack(ProtobufDifferentialDriveFeedforward msg, DifferentialDriveFeedforward value) { + msg.setKvLinear(value.m_kVLinear); + msg.setKaLinear(value.m_kALinear); + msg.setKvAngular(value.m_kVAngular); + msg.setKaAngular(value.m_kAAngular); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProto.java new file mode 100644 index 0000000000..cdd8c137e0 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProto.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.controller.proto; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufSimpleMotorFeedforward; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public final class SimpleMotorFeedforwardProto + implements Protobuf { + @Override + public Class getTypeClass() { + return SimpleMotorFeedforward.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufSimpleMotorFeedforward.getDescriptor(); + } + + @Override + public ProtobufSimpleMotorFeedforward createMessage() { + return ProtobufSimpleMotorFeedforward.newInstance(); + } + + @Override + public SimpleMotorFeedforward unpack(ProtobufSimpleMotorFeedforward msg) { + return new SimpleMotorFeedforward(msg.getKs(), msg.getKv(), msg.getKa(), msg.getDt()); + } + + @Override + public void pack(ProtobufSimpleMotorFeedforward msg, SimpleMotorFeedforward value) { + msg.setKs(value.getKs()).setKv(value.getKv()).setKa(value.getKa()).setDt(value.getDt()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStruct.java new file mode 100644 index 0000000000..1e3e86c3a0 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStruct.java @@ -0,0 +1,49 @@ +// 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.controller.struct; + +import edu.wpi.first.math.controller.DifferentialDriveFeedforward; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public final class DifferentialDriveFeedforwardStruct + implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveFeedforward.class; + } + + @Override + public String getTypeName() { + return "DifferentialDriveFeedforward"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double kVLinear;double kALinear;double kVAngular;double kAAngular"; + } + + @Override + public DifferentialDriveFeedforward unpack(ByteBuffer bb) { + double kVLinear = bb.getDouble(); + double kALinear = bb.getDouble(); + double kVAngular = bb.getDouble(); + double kAAngular = bb.getDouble(); + return new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveFeedforward value) { + bb.putDouble(value.m_kVLinear); + bb.putDouble(value.m_kALinear); + bb.putDouble(value.m_kVAngular); + bb.putDouble(value.m_kAAngular); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStruct.java new file mode 100644 index 0000000000..36ec630e42 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStruct.java @@ -0,0 +1,48 @@ +// 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.controller.struct; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public final class SimpleMotorFeedforwardStruct implements Struct { + @Override + public Class getTypeClass() { + return SimpleMotorFeedforward.class; + } + + @Override + public String getTypeName() { + return "SimpleMotorFeedforward"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double ks;double kv;double ka;double dt"; + } + + @Override + public SimpleMotorFeedforward unpack(ByteBuffer bb) { + double ks = bb.getDouble(); + double kv = bb.getDouble(); + double ka = bb.getDouble(); + double dt = bb.getDouble(); + return new SimpleMotorFeedforward(ks, kv, ka, dt); + } + + @Override + public void pack(ByteBuffer bb, SimpleMotorFeedforward value) { + bb.putDouble(value.getKs()); + bb.putDouble(value.getKv()); + bb.putDouble(value.getKa()); + bb.putDouble(value.getDt()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java index a63eaead1e..8bc91b1d8e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java @@ -4,8 +4,13 @@ package edu.wpi.first.math.kinematics; +import edu.wpi.first.math.kinematics.proto.MecanumDriveMotorVoltagesProto; +import edu.wpi.first.math.kinematics.struct.MecanumDriveMotorVoltagesStruct; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.StructSerializable; + /** Represents the motor voltages for a mecanum drive drivetrain. */ -public class MecanumDriveMotorVoltages { +public class MecanumDriveMotorVoltages implements ProtobufSerializable, StructSerializable { /** Voltage of the front left motor. */ public double frontLeftVoltage; @@ -47,4 +52,11 @@ public class MecanumDriveMotorVoltages { + "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage); } + + /** MecanumDriveMotorVoltages struct for serialization. */ + public static final MecanumDriveMotorVoltagesStruct struct = + new MecanumDriveMotorVoltagesStruct(); + + /** MecanumDriveMotorVoltages protobuf for serialization. */ + public static final MecanumDriveMotorVoltagesProto proto = new MecanumDriveMotorVoltagesProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index a25a2bed7d..99dd85d987 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -12,10 +12,15 @@ import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.proto.SwerveDriveKinematicsProto; +import edu.wpi.first.math.kinematics.struct.SwerveDriveKinematicsStruct; import edu.wpi.first.units.Angle; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Velocity; +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 org.ejml.simple.SimpleMatrix; @@ -41,7 +46,9 @@ import org.ejml.simple.SimpleMatrix; */ @SuppressWarnings("overrides") public class SwerveDriveKinematics - implements Kinematics { + implements Kinematics, + ProtobufSerializable, + StructSerializable { private final SimpleMatrix m_inverseKinematics; private final SimpleMatrix m_forwardKinematics; @@ -415,4 +422,28 @@ public class SwerveDriveKinematics } return newPositions; } + + /** + * Gets the locations of the modules relative to the center of rotation. + * + * @return The locations of the modules relative to the center of rotation. This array should not + * be modified. + */ + @SuppressWarnings("PMD.MethodReturnsInternalArray") + public Translation2d[] getModules() { + return m_modules; + } + + /** SwerveDriveKinematics protobuf for serialization. */ + public static final SwerveDriveKinematicsProto proto = new SwerveDriveKinematicsProto(); + + /** + * Creates an implementation of the {@link Struct} interface for swerve drive kinematics objects. + * + * @param numModules The number of modules of the kinematics objects this serializer processes. + * @return The struct implementation. + */ + public static final SwerveDriveKinematicsStruct getStruct(int numModules) { + return new SwerveDriveKinematicsStruct(numModules); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProto.java new file mode 100644 index 0000000000..0b5f0c2583 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProto.java @@ -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. + +package edu.wpi.first.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveMotorVoltages; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public final class MecanumDriveMotorVoltagesProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MecanumDriveMotorVoltages.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMecanumDriveMotorVoltages.getDescriptor(); + } + + @Override + public ProtobufMecanumDriveMotorVoltages createMessage() { + return ProtobufMecanumDriveMotorVoltages.newInstance(); + } + + @Override + public MecanumDriveMotorVoltages unpack(ProtobufMecanumDriveMotorVoltages msg) { + return new MecanumDriveMotorVoltages( + msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight()); + } + + @Override + public void pack(ProtobufMecanumDriveMotorVoltages msg, MecanumDriveMotorVoltages value) { + msg.setFrontLeft(value.frontLeftVoltage); + msg.setFrontRight(value.frontRightVoltage); + msg.setRearLeft(value.rearLeftVoltage); + msg.setRearRight(value.rearRightVoltage); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProto.java new file mode 100644 index 0000000000..21e7fdc667 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProto.java @@ -0,0 +1,39 @@ +// 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.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveDriveKinematics; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public final class SwerveDriveKinematicsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return SwerveDriveKinematics.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufSwerveDriveKinematics.getDescriptor(); + } + + @Override + public ProtobufSwerveDriveKinematics createMessage() { + return ProtobufSwerveDriveKinematics.newInstance(); + } + + @Override + public SwerveDriveKinematics unpack(ProtobufSwerveDriveKinematics msg) { + return new SwerveDriveKinematics(Protobuf.unpackArray(msg.getModules(), Translation2d.proto)); + } + + @Override + public void pack(ProtobufSwerveDriveKinematics msg, SwerveDriveKinematics value) { + Protobuf.packArray(msg.getMutableModules(), value.getModules(), Translation2d.proto); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStruct.java new file mode 100644 index 0000000000..8078e0a7b1 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStruct.java @@ -0,0 +1,48 @@ +// 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.MecanumDriveMotorVoltages; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public final class MecanumDriveMotorVoltagesStruct implements Struct { + @Override + public Class getTypeClass() { + return MecanumDriveMotorVoltages.class; + } + + @Override + public String getTypeName() { + return "MecanumDriveMotorVoltages"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double front_left;double front_right;double rear_left;double rear_right"; + } + + @Override + public MecanumDriveMotorVoltages unpack(ByteBuffer bb) { + double front_left = bb.getDouble(); + double front_right = bb.getDouble(); + double rear_left = bb.getDouble(); + double rear_right = bb.getDouble(); + return new MecanumDriveMotorVoltages(front_left, front_right, rear_left, rear_right); + } + + @Override + public void pack(ByteBuffer bb, MecanumDriveMotorVoltages value) { + bb.putDouble(value.frontLeftVoltage); + bb.putDouble(value.frontRightVoltage); + bb.putDouble(value.rearLeftVoltage); + bb.putDouble(value.rearRightVoltage); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStruct.java new file mode 100644 index 0000000000..2f1cbc0cbe --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStruct.java @@ -0,0 +1,58 @@ +// 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.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public final class SwerveDriveKinematicsStruct implements Struct { + private final int m_numModules; + + /** + * Constructs the {@link Struct} implementation. + * + * @param numModules the number of modules of the kinematics objects this serializer processes. + */ + public SwerveDriveKinematicsStruct(int numModules) { + m_numModules = numModules; + } + + @Override + public Class getTypeClass() { + return SwerveDriveKinematics.class; + } + + @Override + public String getTypeName() { + return "SwerveDriveKinematics__" + m_numModules; + } + + @Override + public int getSize() { + return Translation2d.struct.getSize() * m_numModules; + } + + @Override + public String getSchema() { + return "Translation2d modules[" + m_numModules + "]"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation2d.struct}; + } + + @Override + public SwerveDriveKinematics unpack(ByteBuffer bb) { + return new SwerveDriveKinematics(Struct.unpackArray(bb, m_numModules, Translation2d.struct)); + } + + @Override + public void pack(ByteBuffer bb, SwerveDriveKinematics value) { + Struct.packArray(bb, value.getModules(), Translation2d.struct); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/proto/MatrixProto.java b/wpimath/src/main/java/edu/wpi/first/math/proto/MatrixProto.java new file mode 100644 index 0000000000..37d75a9d23 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/proto/MatrixProto.java @@ -0,0 +1,73 @@ +// 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.proto; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.proto.Wpimath.ProtobufMatrix; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class MatrixProto + implements Protobuf, ProtobufMatrix> { + private final Nat m_rows; + private final Nat m_cols; + + /** + * Constructs the {@link Protobuf} implementation. + * + * @param rows The number of rows of the matrices this serializer processes. + * @param cols The number of cols of the matrices this serializer processes. + */ + public MatrixProto(Nat rows, Nat cols) { + m_rows = rows; + m_cols = cols; + } + + @Override + public Class> getTypeClass() { + @SuppressWarnings("unchecked") + var clazz = (Class>) (Class) Matrix.class; + return clazz; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMatrix.getDescriptor(); + } + + @Override + public ProtobufMatrix createMessage() { + return ProtobufMatrix.newInstance(); + } + + @Override + public Matrix unpack(ProtobufMatrix msg) { + if (msg.getNumRows() != m_rows.getNum() || msg.getNumCols() != m_cols.getNum()) { + throw new IllegalArgumentException( + "Tried to unpack msg " + + msg + + " with " + + msg.getNumRows() + + " rows and " + + msg.getNumCols() + + " columns into Matrix with " + + m_rows.getNum() + + " rows and " + + m_cols.getNum() + + " columns"); + } + return MatBuilder.fill(m_rows, m_cols, Protobuf.unpackArray(msg.getData())); + } + + @Override + public void pack(ProtobufMatrix msg, Matrix value) { + msg.setNumRows(value.getNumRows()); + msg.setNumCols(value.getNumCols()); + Protobuf.packArray(msg.getMutableData(), value.getData()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/proto/VectorProto.java b/wpimath/src/main/java/edu/wpi/first/math/proto/VectorProto.java new file mode 100644 index 0000000000..9fe68e5ee5 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/proto/VectorProto.java @@ -0,0 +1,64 @@ +// 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.proto; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.proto.Wpimath.ProtobufVector; +import edu.wpi.first.util.protobuf.Protobuf; +import org.ejml.simple.SimpleMatrix; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public final class VectorProto implements Protobuf, ProtobufVector> { + private final Nat m_rows; + + /** + * Constructs the {@link Protobuf} implementation. + * + * @param rows The number of rows of the vectors this serializer processes. + */ + public VectorProto(Nat rows) { + m_rows = rows; + } + + @Override + public Class> getTypeClass() { + @SuppressWarnings("unchecked") + var clazz = (Class>) (Class) Vector.class; + return clazz; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufVector.getDescriptor(); + } + + @Override + public ProtobufVector createMessage() { + return ProtobufVector.newInstance(); + } + + @Override + public Vector unpack(ProtobufVector msg) { + if (msg.getRows().length() != m_rows.getNum()) { + throw new IllegalArgumentException( + "Tried to unpack msg " + + msg + + " with " + + msg.getRows().length() + + " rows into Vector with " + + m_rows.getNum() + + " rows"); + } + var storage = new SimpleMatrix(Protobuf.unpackArray(msg.getRows())); + return new Vector(storage); + } + + @Override + public void pack(ProtobufVector msg, Vector value) { + Protobuf.packArray(msg.getMutableRows(), value.getData()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java index f9457bccdb..bbe01b004b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java @@ -4,13 +4,29 @@ package edu.wpi.first.math.spline; +import edu.wpi.first.math.spline.proto.CubicHermiteSplineProto; +import edu.wpi.first.math.spline.struct.CubicHermiteSplineStruct; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.StructSerializable; import org.ejml.simple.SimpleMatrix; /** Represents a hermite spline of degree 3. */ -public class CubicHermiteSpline extends Spline { +public class CubicHermiteSpline extends Spline implements ProtobufSerializable, StructSerializable { private static SimpleMatrix hermiteBasis; private final SimpleMatrix m_coefficients; + /** The control vector for the initial point in the x dimension. DO NOT MODIFY THIS ARRAY! */ + public final double[] xInitialControlVector; + + /** The control vector for the final point in the x dimension. DO NOT MODIFY THIS ARRAY! */ + public final double[] xFinalControlVector; + + /** The control vector for the initial point in the y dimension. DO NOT MODIFY THIS ARRAY! */ + public final double[] yInitialControlVector; + + /** The control vector for the final point in the y dimension. DO NOT MODIFY THIS ARRAY! */ + public final double[] yFinalControlVector; + private final ControlVector m_initialControlVector; private final ControlVector m_finalControlVector; @@ -23,12 +39,17 @@ public class CubicHermiteSpline extends Spline { * @param yInitialControlVector The control vector for the initial point in the y dimension. * @param yFinalControlVector The control vector for the final point in the y dimension. */ + @SuppressWarnings("PMD.ArrayIsStoredDirectly") public CubicHermiteSpline( double[] xInitialControlVector, double[] xFinalControlVector, double[] yInitialControlVector, double[] yFinalControlVector) { super(3); + this.xInitialControlVector = xInitialControlVector; + this.xFinalControlVector = xFinalControlVector; + this.yInitialControlVector = yInitialControlVector; + this.yFinalControlVector = yFinalControlVector; // Populate the coefficients for the actual spline equations. // Row 0 is x coefficients @@ -163,4 +184,10 @@ public class CubicHermiteSpline extends Spline { finalVector[0], finalVector[1] }); } + + /** CubicHermiteSpline struct for serialization. */ + public static final CubicHermiteSplineProto proto = new CubicHermiteSplineProto(); + + /** CubicHermiteSpline protobuf for serialization. */ + public static final CubicHermiteSplineStruct struct = new CubicHermiteSplineStruct(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java index 0dc712a3b8..c119c026a3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java @@ -4,13 +4,30 @@ package edu.wpi.first.math.spline; +import edu.wpi.first.math.spline.proto.QuinticHermiteSplineProto; +import edu.wpi.first.math.spline.struct.QuinticHermiteSplineStruct; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.StructSerializable; import org.ejml.simple.SimpleMatrix; /** Represents a hermite spline of degree 5. */ -public class QuinticHermiteSpline extends Spline { +public class QuinticHermiteSpline extends Spline + implements ProtobufSerializable, StructSerializable { private static SimpleMatrix hermiteBasis; private final SimpleMatrix m_coefficients; + /** The control vector for the initial point in the x dimension. DO NOT MODIFY THIS ARRAY! */ + public final double[] xInitialControlVector; + + /** The control vector for the final point in the x dimension. DO NOT MODIFY THIS ARRAY! */ + public final double[] xFinalControlVector; + + /** The control vector for the initial point in the y dimension. DO NOT MODIFY THIS ARRAY! */ + public final double[] yInitialControlVector; + + /** The control vector for the final point in the y dimension. DO NOT MODIFY THIS ARRAY! */ + public final double[] yFinalControlVector; + private final ControlVector m_initialControlVector; private final ControlVector m_finalControlVector; @@ -23,12 +40,17 @@ public class QuinticHermiteSpline extends Spline { * @param yInitialControlVector The control vector for the initial point in the y dimension. * @param yFinalControlVector The control vector for the final point in the y dimension. */ + @SuppressWarnings("PMD.ArrayIsStoredDirectly") public QuinticHermiteSpline( double[] xInitialControlVector, double[] xFinalControlVector, double[] yInitialControlVector, double[] yFinalControlVector) { super(5); + this.xInitialControlVector = xInitialControlVector; + this.yInitialControlVector = yInitialControlVector; + this.xFinalControlVector = xFinalControlVector; + this.yFinalControlVector = yFinalControlVector; // Populate the coefficients for the actual spline equations. // Row 0 is x coefficients @@ -171,4 +193,10 @@ public class QuinticHermiteSpline extends Spline { finalVector[0], finalVector[1], finalVector[2] }); } + + /** QuinticHermiteSpline struct for serialization. */ + public static final QuinticHermiteSplineProto proto = new QuinticHermiteSplineProto(); + + /** QuinticHermiteSpline protobuf for serialization. */ + public static final QuinticHermiteSplineStruct struct = new QuinticHermiteSplineStruct(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/proto/CubicHermiteSplineProto.java b/wpimath/src/main/java/edu/wpi/first/math/spline/proto/CubicHermiteSplineProto.java new file mode 100644 index 0000000000..a969e5b288 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/proto/CubicHermiteSplineProto.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.spline.proto; + +import edu.wpi.first.math.proto.Spline.ProtobufCubicHermiteSpline; +import edu.wpi.first.math.spline.CubicHermiteSpline; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public final class CubicHermiteSplineProto + implements Protobuf { + @Override + public Class getTypeClass() { + return CubicHermiteSpline.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufCubicHermiteSpline.getDescriptor(); + } + + @Override + public ProtobufCubicHermiteSpline createMessage() { + return ProtobufCubicHermiteSpline.newInstance(); + } + + @Override + public CubicHermiteSpline unpack(ProtobufCubicHermiteSpline msg) { + return new CubicHermiteSpline( + Protobuf.unpackArray(msg.getXInitial()), + Protobuf.unpackArray(msg.getXFinal()), + Protobuf.unpackArray(msg.getYInitial()), + Protobuf.unpackArray(msg.getYFinal())); + } + + @Override + public void pack(ProtobufCubicHermiteSpline msg, CubicHermiteSpline value) { + Protobuf.packArray(msg.getMutableXInitial(), value.xInitialControlVector); + Protobuf.packArray(msg.getMutableXFinal(), value.xFinalControlVector); + Protobuf.packArray(msg.getMutableYInitial(), value.yInitialControlVector); + Protobuf.packArray(msg.getMutableYFinal(), value.yFinalControlVector); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/proto/QuinticHermiteSplineProto.java b/wpimath/src/main/java/edu/wpi/first/math/spline/proto/QuinticHermiteSplineProto.java new file mode 100644 index 0000000000..93f042214b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/proto/QuinticHermiteSplineProto.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.spline.proto; + +import edu.wpi.first.math.proto.Spline.ProtobufQuinticHermiteSpline; +import edu.wpi.first.math.spline.QuinticHermiteSpline; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public final class QuinticHermiteSplineProto + implements Protobuf { + @Override + public Class getTypeClass() { + return QuinticHermiteSpline.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufQuinticHermiteSpline.getDescriptor(); + } + + @Override + public ProtobufQuinticHermiteSpline createMessage() { + return ProtobufQuinticHermiteSpline.newInstance(); + } + + @Override + public QuinticHermiteSpline unpack(ProtobufQuinticHermiteSpline msg) { + return new QuinticHermiteSpline( + Protobuf.unpackArray(msg.getXInitial()), + Protobuf.unpackArray(msg.getXFinal()), + Protobuf.unpackArray(msg.getYInitial()), + Protobuf.unpackArray(msg.getYFinal())); + } + + @Override + public void pack(ProtobufQuinticHermiteSpline msg, QuinticHermiteSpline value) { + Protobuf.packArray(msg.getMutableXInitial(), value.xInitialControlVector); + Protobuf.packArray(msg.getMutableXFinal(), value.xFinalControlVector); + Protobuf.packArray(msg.getMutableYInitial(), value.yInitialControlVector); + Protobuf.packArray(msg.getMutableYFinal(), value.yFinalControlVector); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/struct/CubicHermiteSplineStruct.java b/wpimath/src/main/java/edu/wpi/first/math/spline/struct/CubicHermiteSplineStruct.java new file mode 100644 index 0000000000..9c60642aca --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/struct/CubicHermiteSplineStruct.java @@ -0,0 +1,48 @@ +// 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.spline.struct; + +import edu.wpi.first.math.spline.CubicHermiteSpline; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public final class CubicHermiteSplineStruct implements Struct { + @Override + public Class getTypeClass() { + return CubicHermiteSpline.class; + } + + @Override + public String getTypeName() { + return "CubicHermiteSpline"; + } + + @Override + public int getSize() { + return kSizeDouble * 4 * 2; + } + + @Override + public String getSchema() { + return "double xInitial[2];double xFinal[2];double yInitial[2];double yFinal[2]"; + } + + @Override + public CubicHermiteSpline unpack(ByteBuffer bb) { + return new CubicHermiteSpline( + Struct.unpackDoubleArray(bb, 2), + Struct.unpackDoubleArray(bb, 2), + Struct.unpackDoubleArray(bb, 2), + Struct.unpackDoubleArray(bb, 2)); + } + + @Override + public void pack(ByteBuffer bb, CubicHermiteSpline value) { + Struct.packArray(bb, value.xInitialControlVector); + Struct.packArray(bb, value.xFinalControlVector); + Struct.packArray(bb, value.yInitialControlVector); + Struct.packArray(bb, value.yFinalControlVector); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/struct/QuinticHermiteSplineStruct.java b/wpimath/src/main/java/edu/wpi/first/math/spline/struct/QuinticHermiteSplineStruct.java new file mode 100644 index 0000000000..304cd6d8cf --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/struct/QuinticHermiteSplineStruct.java @@ -0,0 +1,48 @@ +// 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.spline.struct; + +import edu.wpi.first.math.spline.QuinticHermiteSpline; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public final class QuinticHermiteSplineStruct implements Struct { + @Override + public Class getTypeClass() { + return QuinticHermiteSpline.class; + } + + @Override + public String getTypeName() { + return "QuinticHermiteSpline"; + } + + @Override + public int getSize() { + return kSizeDouble * 4 * 3; + } + + @Override + public String getSchema() { + return "double xInitial[3];double xFinal[3];double yInitial[3];double yFinal[3]"; + } + + @Override + public QuinticHermiteSpline unpack(ByteBuffer bb) { + return new QuinticHermiteSpline( + Struct.unpackDoubleArray(bb, 3), + Struct.unpackDoubleArray(bb, 3), + Struct.unpackDoubleArray(bb, 3), + Struct.unpackDoubleArray(bb, 3)); + } + + @Override + public void pack(ByteBuffer bb, QuinticHermiteSpline value) { + Struct.packArray(bb, value.xInitialControlVector); + Struct.packArray(bb, value.xFinalControlVector); + Struct.packArray(bb, value.yInitialControlVector); + Struct.packArray(bb, value.yFinalControlVector); + } +} 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 new file mode 100644 index 0000000000..3fb2eb822a --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/struct/MatrixStruct.java @@ -0,0 +1,61 @@ +// 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.struct; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public final class MatrixStruct implements Struct> { + private final Nat m_rows; + private final Nat m_cols; + + /** + * Constructs the {@link Struct} implementation. + * + * @param rows The number of rows of the matrices this serializer processes. + * @param cols The number of cols of the matrices this serializer processes. + */ + public MatrixStruct(Nat rows, Nat cols) { + m_rows = rows; + m_cols = cols; + } + + @Override + public Class> getTypeClass() { + @SuppressWarnings("unchecked") + var clazz = (Class>) (Class) Matrix.class; + return clazz; + } + + @Override + public String getTypeName() { + return "Matrix__" + m_rows.getNum() + "_" + m_cols.getNum(); + } + + @Override + public int getSize() { + return kSizeDouble * m_rows.getNum() * m_cols.getNum(); + } + + @Override + public String getSchema() { + return "double data[" + (m_rows.getNum() * m_cols.getNum()) + "]"; + } + + @Override + public Matrix unpack(ByteBuffer bb) { + return MatBuilder.fill( + m_rows, m_cols, Struct.unpackDoubleArray(bb, m_rows.getNum() * m_cols.getNum())); + } + + @Override + public void pack(ByteBuffer bb, Matrix value) { + Struct.packArray(bb, value.getData()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/struct/VectorStruct.java b/wpimath/src/main/java/edu/wpi/first/math/struct/VectorStruct.java new file mode 100644 index 0000000000..3dd19156f3 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/struct/VectorStruct.java @@ -0,0 +1,58 @@ +// 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.struct; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.Vector; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; +import org.ejml.simple.SimpleMatrix; + +public final class VectorStruct implements Struct> { + private final int m_rows; + + /** + * Constructs the {@link Struct} implementation. + * + * @param rows The number of rows of the vectors this serializer processes. + */ + public VectorStruct(Nat rows) { + m_rows = rows.getNum(); + } + + @Override + public Class> getTypeClass() { + @SuppressWarnings("unchecked") + var clazz = (Class>) (Class) Vector.class; + return clazz; + } + + @Override + public String getTypeName() { + return "Vector__" + m_rows; + } + + @Override + public int getSize() { + return kSizeDouble * m_rows; + } + + @Override + public String getSchema() { + return "double data[" + m_rows + "]"; + } + + @Override + public Vector unpack(ByteBuffer bb) { + var storage = new SimpleMatrix(Struct.unpackDoubleArray(bb, m_rows)); + return new Vector(storage); + } + + @Override + public void pack(ByteBuffer bb, Vector value) { + Struct.packArray(bb, value.getData()); + } +} 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 923f11e48f..2b29d1954a 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 @@ -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 Number of inputs. * @param Number of outputs. */ -public class LinearSystem { +public class LinearSystem + implements ProtobufSerializable, StructSerializable { /** Continuous system matrix. */ private final Matrix m_A; @@ -361,4 +369,38 @@ public class LinearSystem 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 protobuf implementation. + */ + public static + 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/proto/LinearSystemProto.java b/wpimath/src/main/java/edu/wpi/first/math/system/proto/LinearSystemProto.java new file mode 100644 index 0000000000..aaf11fec06 --- /dev/null +++ b/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 + implements Protobuf, ProtobufLinearSystem> { + private final Nat m_states; + private final Nat m_inputs; + private final Nat m_outputs; + private final Protobuf, ProtobufMatrix> m_AProto; + private final Protobuf, ProtobufMatrix> m_BProto; + private final Protobuf, ProtobufMatrix> m_CProto; + private final Protobuf, 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, Nat inputs, Nat 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> getTypeClass() { + @SuppressWarnings("unchecked") + var clazz = (Class>) (Class) LinearSystem.class; + return clazz; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufLinearSystem.getDescriptor(); + } + + @Override + public ProtobufLinearSystem createMessage() { + return ProtobufLinearSystem.newInstance(); + } + + @Override + public LinearSystem 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 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()); + } +} 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 0000000000..cf675ac1ae --- /dev/null +++ b/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 + implements Struct> { + private final int m_states; + private final int m_inputs; + private final int 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.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> getTypeClass() { + @SuppressWarnings("unchecked") + var clazz = (Class>) (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 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/cpp/controller/DifferentialDriveFeedforward.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp index 257d7e1efc..e458526cb6 100644 --- a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp +++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp @@ -15,14 +15,22 @@ DifferentialDriveFeedforward::DifferentialDriveFeedforward( decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, decltype(1_V / 1_rad_per_s) kVAngular, decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) - : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem( - kVLinear, kALinear, kVAngular, kAAngular, trackwidth)} {} + // See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps), + // decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V / + // 1_rad_per_s_sq)) + : DifferentialDriveFeedforward{kVLinear, kALinear, + kVAngular * 2.0 / trackwidth * 1_rad, + kAAngular * 2.0 / trackwidth * 1_rad} {} DifferentialDriveFeedforward::DifferentialDriveFeedforward( decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem( - kVLinear, kALinear, kVAngular, kAAngular)} {} + kVLinear, kALinear, kVAngular, kAAngular)}, + m_kVLinear{kVLinear}, + m_kALinear{kALinear}, + m_kVAngular{kVAngular}, + m_kAAngular{kAAngular} {} DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate( units::meters_per_second_t currentLeftVelocity, diff --git a/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveFeedforwardProto.cpp b/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveFeedforwardProto.cpp new file mode 100644 index 0000000000..4a5876d93a --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveFeedforwardProto.cpp @@ -0,0 +1,36 @@ +// 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/controller/proto/DifferentialDriveFeedforwardProto.h" + +#include + +#include "controller.pb.h" + +google::protobuf::Message* wpi::Protobuf< + frc::DifferentialDriveFeedforward>::New(google::protobuf::Arena* arena) { + return wpi::CreateMessage( + arena); +} + +frc::DifferentialDriveFeedforward +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast( + &msg); + return {decltype(1_V / 1_mps){m->kv_linear()}, + decltype(1_V / 1_mps_sq){m->ka_linear()}, + decltype(1_V / 1_mps){m->kv_angular()}, + decltype(1_V / 1_mps_sq){m->ka_angular()}}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::DifferentialDriveFeedforward& value) { + auto m = static_cast(msg); + m->set_kv_linear(value.m_kVLinear.value()); + m->set_ka_linear(value.m_kALinear.value()); + m->set_kv_angular(value.m_kVAngular.value()); + m->set_ka_angular(value.m_kAAngular.value()); +} diff --git a/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveFeedforwardStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveFeedforwardStruct.cpp new file mode 100644 index 0000000000..51a25d9c43 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveFeedforwardStruct.cpp @@ -0,0 +1,29 @@ +// 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/controller/struct/DifferentialDriveFeedforwardStruct.h" + +namespace { +constexpr size_t kKvLinearOff = 0; +constexpr size_t kKaLinearOff = kKvLinearOff + 8; +constexpr size_t kKvAngularOff = kKaLinearOff + 8; +constexpr size_t kKaAngularOff = kKvAngularOff + 8; +} // namespace + +frc::DifferentialDriveFeedforward wpi::Struct< + frc::DifferentialDriveFeedforward>::Unpack(std::span data) { + return { + decltype(1_V / 1_mps){wpi::UnpackStruct(data)}, + decltype(1_V / 1_mps_sq){wpi::UnpackStruct(data)}, + decltype(1_V / 1_mps){wpi::UnpackStruct(data)}, + decltype(1_V / 1_mps_sq){wpi::UnpackStruct(data)}}; +} + +void wpi::Struct::Pack( + std::span data, const frc::DifferentialDriveFeedforward& value) { + wpi::PackStruct(data, value.m_kVLinear.value()); + wpi::PackStruct(data, value.m_kALinear.value()); + wpi::PackStruct(data, value.m_kVAngular.value()); + wpi::PackStruct(data, value.m_kAAngular.value()); +} diff --git a/wpimath/src/main/native/cpp/spline/proto/CubicHermiteSplineProto.cpp b/wpimath/src/main/native/cpp/spline/proto/CubicHermiteSplineProto.cpp new file mode 100644 index 0000000000..6189d4458c --- /dev/null +++ b/wpimath/src/main/native/cpp/spline/proto/CubicHermiteSplineProto.cpp @@ -0,0 +1,35 @@ +// 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/spline/proto/CubicHermiteSplineProto.h" + +#include + +#include "spline.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return wpi::CreateMessage(arena); +} + +frc::CubicHermiteSpline wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::CubicHermiteSpline{ + wpi::UnpackProtobufArray(m->x_initial()), + wpi::UnpackProtobufArray(m->x_final()), + wpi::UnpackProtobufArray(m->y_initial()), + wpi::UnpackProtobufArray(m->y_final())}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::CubicHermiteSpline& value) { + auto m = static_cast(msg); + wpi::PackProtobufArray(m->mutable_x_initial(), + value.GetInitialControlVector().x); + wpi::PackProtobufArray(m->mutable_x_final(), value.GetFinalControlVector().x); + wpi::PackProtobufArray(m->mutable_y_initial(), + value.GetInitialControlVector().y); + wpi::PackProtobufArray(m->mutable_y_final(), value.GetFinalControlVector().y); +} diff --git a/wpimath/src/main/native/cpp/spline/proto/QuinticHermiteSplineProto.cpp b/wpimath/src/main/native/cpp/spline/proto/QuinticHermiteSplineProto.cpp new file mode 100644 index 0000000000..c4d8ffc851 --- /dev/null +++ b/wpimath/src/main/native/cpp/spline/proto/QuinticHermiteSplineProto.cpp @@ -0,0 +1,35 @@ +// 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/spline/proto/QuinticHermiteSplineProto.h" + +#include + +#include "spline.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return wpi::CreateMessage(arena); +} + +frc::QuinticHermiteSpline wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::QuinticHermiteSpline{ + wpi::UnpackProtobufArray(m->x_initial()), + wpi::UnpackProtobufArray(m->x_final()), + wpi::UnpackProtobufArray(m->y_initial()), + wpi::UnpackProtobufArray(m->y_final())}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::QuinticHermiteSpline& value) { + auto m = static_cast(msg); + wpi::PackProtobufArray(m->mutable_x_initial(), + value.GetInitialControlVector().x); + wpi::PackProtobufArray(m->mutable_x_final(), value.GetFinalControlVector().x); + wpi::PackProtobufArray(m->mutable_y_initial(), + value.GetInitialControlVector().y); + wpi::PackProtobufArray(m->mutable_y_final(), value.GetFinalControlVector().y); +} diff --git a/wpimath/src/main/native/cpp/spline/struct/CubicHermiteSplineStruct.cpp b/wpimath/src/main/native/cpp/spline/struct/CubicHermiteSplineStruct.cpp new file mode 100644 index 0000000000..3d837cd89c --- /dev/null +++ b/wpimath/src/main/native/cpp/spline/struct/CubicHermiteSplineStruct.cpp @@ -0,0 +1,31 @@ +// 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/spline/struct/CubicHermiteSplineStruct.h" + +namespace { +constexpr size_t kXInitialOff = 0; +constexpr size_t kXFinalOff = kXInitialOff + 2 * 8; +constexpr size_t kYInitialOff = kXFinalOff + 2 * 8; +constexpr size_t kYFinalOff = kYInitialOff + 2 * 8; +} // namespace + +frc::CubicHermiteSpline wpi::Struct::Unpack( + std::span data) { + return frc::CubicHermiteSpline{ + wpi::UnpackStructArray(data), + wpi::UnpackStructArray(data), + wpi::UnpackStructArray(data), + wpi::UnpackStructArray(data)}; +} + +void wpi::Struct::Pack( + std::span data, const frc::CubicHermiteSpline& value) { + wpi::PackStructArray(data, + value.GetInitialControlVector().x); + wpi::PackStructArray(data, value.GetFinalControlVector().x); + wpi::PackStructArray(data, + value.GetInitialControlVector().y); + wpi::PackStructArray(data, value.GetFinalControlVector().y); +} diff --git a/wpimath/src/main/native/cpp/spline/struct/QuinticHermiteSplineStruct.cpp b/wpimath/src/main/native/cpp/spline/struct/QuinticHermiteSplineStruct.cpp new file mode 100644 index 0000000000..fd1906718c --- /dev/null +++ b/wpimath/src/main/native/cpp/spline/struct/QuinticHermiteSplineStruct.cpp @@ -0,0 +1,31 @@ +// 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/spline/struct/QuinticHermiteSplineStruct.h" + +namespace { +constexpr size_t kXInitialOff = 0; +constexpr size_t kXFinalOff = kXInitialOff + 3 * 8; +constexpr size_t kYInitialOff = kXFinalOff + 3 * 8; +constexpr size_t kYFinalOff = kYInitialOff + 3 * 8; +} // namespace + +frc::QuinticHermiteSpline wpi::Struct::Unpack( + std::span data) { + return frc::QuinticHermiteSpline{ + wpi::UnpackStructArray(data), + wpi::UnpackStructArray(data), + wpi::UnpackStructArray(data), + wpi::UnpackStructArray(data)}; +} + +void wpi::Struct::Pack( + std::span data, const frc::QuinticHermiteSpline& value) { + wpi::PackStructArray(data, + value.GetInitialControlVector().x); + wpi::PackStructArray(data, value.GetFinalControlVector().x); + wpi::PackStructArray(data, + value.GetInitialControlVector().y); + wpi::PackStructArray(data, value.GetFinalControlVector().y); +} diff --git a/wpimath/src/main/native/exports.def b/wpimath/src/main/native/exports.def new file mode 100644 index 0000000000..c6cd02dd64 --- /dev/null +++ b/wpimath/src/main/native/exports.def @@ -0,0 +1,9 @@ +EXPORTS + ??$CreateMaybeMessage@VProtobufSimpleMotorFeedforward@proto@wpi@@$$V@Arena@protobuf@google@@CAPEAVProtobufSimpleMotorFeedforward@proto@wpi@@PEAV012@@Z + ??$CreateMaybeMessage@VProtobufTranslation2d@proto@wpi@@$$V@Arena@protobuf@google@@CAPEAVProtobufTranslation2d@proto@wpi@@PEAV012@@Z + ?Clear@ProtobufTranslation2d@proto@wpi@@UEAAXXZ + ??$CreateMaybeMessage@VProtobufSwerveDriveKinematics@proto@wpi@@$$V@Arena@protobuf@google@@CAPEAVProtobufSwerveDriveKinematics@proto@wpi@@PEAV012@@Z + ??$CreateMaybeMessage@VProtobufMatrix@proto@wpi@@$$V@Arena@protobuf@google@@CAPEAVProtobufMatrix@proto@wpi@@PEAV012@@Z + ??$CreateMaybeMessage@VProtobufVector@proto@wpi@@$$V@Arena@protobuf@google@@CAPEAVProtobufVector@proto@wpi@@PEAV012@@Z + ??$CreateMaybeMessage@VProtobufLinearSystem@proto@wpi@@$$V@Arena@protobuf@google@@CAPEAVProtobufLinearSystem@proto@wpi@@PEAV012@@Z + ?_ProtobufMatrix_default_instance_@proto@wpi@@3UProtobufMatrixDefaultTypeInternal@12@A diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h index 716bc781ad..be7e5b4fac 100644 --- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h @@ -79,5 +79,13 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward { units::meters_per_second_t nextLeftVelocity, units::meters_per_second_t currentRightVelocity, units::meters_per_second_t nextRightVelocity, units::second_t dt); + + decltype(1_V / 1_mps) m_kVLinear; + decltype(1_V / 1_mps_sq) m_kALinear; + decltype(1_V / 1_mps) m_kVAngular; + decltype(1_V / 1_mps_sq) m_kAAngular; }; } // namespace frc + +#include "frc/controller/proto/DifferentialDriveFeedforwardProto.h" +#include "frc/controller/struct/DifferentialDriveFeedforwardStruct.h" diff --git a/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveFeedforwardProto.h b/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveFeedforwardProto.h new file mode 100644 index 0000000000..916e0d1432 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveFeedforwardProto.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 +#include + +#include "frc/controller/DifferentialDriveFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DifferentialDriveFeedforward Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::DifferentialDriveFeedforward& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h b/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h new file mode 100644 index 0000000000..ad57d132d7 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h @@ -0,0 +1,24 @@ +// 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 "frc/controller/SimpleMotorFeedforward.h" +#include "units/length.h" + +// Everything is converted into units for +// frc::SimpleMotorFeedforward + +template +struct wpi::Protobuf> { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::SimpleMotorFeedforward Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::SimpleMotorFeedforward& value); +}; + +#include "frc/controller/proto/SimpleMotorFeedforwardProto.inc" diff --git a/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.inc b/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.inc new file mode 100644 index 0000000000..8cda505a3d --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.inc @@ -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. + +#pragma once + +#include + +#include "controller.pb.h" +#include "frc/controller/proto/SimpleMotorFeedforwardProto.h" + +template +google::protobuf::Message* +wpi::Protobuf>::New( + google::protobuf::Arena* arena) { + return wpi::CreateMessage(arena); +} + +template +frc::SimpleMotorFeedforward +wpi::Protobuf>::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return {units::volt_t{m->ks()}, + units::unit_t::kv_unit>{ + m->kv()}, + units::unit_t::ka_unit>{ + m->ka()}, + units::second_t{m->dt()}}; +} + +template +void wpi::Protobuf>::Pack( + google::protobuf::Message* msg, + const frc::SimpleMotorFeedforward& value) { + auto m = static_cast(msg); + m->set_ks(value.GetKs().value()); + m->set_kv(units::unit_t::kv_unit>{ + value.GetKv()} + .value()); + m->set_ka(units::unit_t::ka_unit>{ + value.GetKa()} + .value()); + m->set_dt(units::second_t{value.GetDt()}.value()); +} diff --git a/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveFeedforwardStruct.h b/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveFeedforwardStruct.h new file mode 100644 index 0000000000..8a6eec799b --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveFeedforwardStruct.h @@ -0,0 +1,29 @@ +// 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 "frc/controller/DifferentialDriveFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeName() { + return "DifferentialDriveFeedforward"; + } + static constexpr size_t GetSize() { return 32; } + static constexpr std::string_view GetSchema() { + return "double kv_linear;double ka_linear;double kv_angular;double " + "ka_angular"; + } + + static frc::DifferentialDriveFeedforward Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveFeedforward& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h new file mode 100644 index 0000000000..bcd8ffc936 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h @@ -0,0 +1,36 @@ +// 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 "frc/controller/SimpleMotorFeedforward.h" +#include "units/length.h" + +// Everything is converted into units for +// frc::SimpleMotorFeedforward + +template +struct wpi::Struct> { + static constexpr std::string_view GetTypeName() { + return "SimpleMotorFeedforward"; + } + static constexpr size_t GetSize() { return 32; } + static constexpr std::string_view GetSchema() { + return "double ks;double kv;double ka;double dt"; + } + + static frc::SimpleMotorFeedforward Unpack( + std::span data); + static void Pack(std::span data, + const frc::SimpleMotorFeedforward& value); +}; + +static_assert( + wpi::StructSerializable>); +static_assert( + wpi::StructSerializable>); + +#include "frc/controller/struct/SimpleMotorFeedforwardStruct.inc" diff --git a/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.inc b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.inc new file mode 100644 index 0000000000..50c6760f95 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.inc @@ -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. + +#pragma once + +#include "frc/controller/struct/SimpleMotorFeedforwardStruct.h" + +template +frc::SimpleMotorFeedforward +wpi::Struct>::Unpack( + std::span data) { + constexpr size_t kKsOff = 0; + constexpr size_t kKvOff = kKsOff + 8; + constexpr size_t kKaOff = kKvOff + 8; + constexpr size_t kDtOff = kKaOff + 8; + return {units::volt_t{wpi::UnpackStruct(data)}, + units::unit_t::kv_unit>{ + wpi::UnpackStruct(data)}, + units::unit_t::ka_unit>{ + wpi::UnpackStruct(data)}, + units::second_t{wpi::UnpackStruct(data)}}; +} + +template +void wpi::Struct>::Pack( + std::span data, + const frc::SimpleMotorFeedforward& value) { + constexpr size_t kKsOff = 0; + constexpr size_t kKvOff = kKsOff + 8; + constexpr size_t kKaOff = kKvOff + 8; + constexpr size_t kDtOff = kKaOff + 8; + wpi::PackStruct(data, value.GetKs().value()); + wpi::PackStruct( + data, + units::unit_t::kv_unit>{ + value.GetKv()} + .value()); + wpi::PackStruct( + data, + units::unit_t::ka_unit>{ + value.GetKa()} + .value()); + wpi::PackStruct(data, units::second_t{value.GetDt()}.value()); +} diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index da8be0b9b6..98eea68e30 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -299,10 +299,14 @@ class SwerveDriveKinematics return {result}; } + const wpi::array GetModules() const { + return m_modules; + } + private: + wpi::array m_modules; mutable Matrixd m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; - wpi::array m_modules; mutable wpi::array m_moduleHeadings; mutable Translation2d m_previousCoR; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.h new file mode 100644 index 0000000000..b84343459d --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.h @@ -0,0 +1,20 @@ +// 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 "frc/kinematics/SwerveDriveKinematics.h" + +template +struct wpi::Protobuf> { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::SwerveDriveKinematics Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::SwerveDriveKinematics& value); +}; + +#include "frc/kinematics/proto/SwerveDriveKinematicsProto.inc" diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.inc b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.inc new file mode 100644 index 0000000000..0a9925481d --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.inc @@ -0,0 +1,44 @@ +// 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/kinematics/proto/SwerveDriveKinematicsProto.h" +#include "kinematics.pb.h" + +template +google::protobuf::Message* +wpi::Protobuf>::New( + google::protobuf::Arena* arena) { + return wpi::CreateMessage(arena); +} + +template +frc::SwerveDriveKinematics +wpi::Protobuf>::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + if (m->modules_size() != NumModules) { + throw std::invalid_argument( + fmt::format("Tried to unpack message with {} elements in modules into " + "SwerveDriveKinematics with {} modules", + m->modules_size(), NumModules)); + } + return frc::SwerveDriveKinematics{ + wpi::UnpackProtobufArray(m->modules())}; +} + +template +void wpi::Protobuf>::Pack( + google::protobuf::Message* msg, + const frc::SwerveDriveKinematics& value) { + auto m = static_cast(msg); + wpi::PackProtobufArray(m->mutable_modules(), value.GetModules()); +} diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.h new file mode 100644 index 0000000000..139b78dbdc --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.h @@ -0,0 +1,41 @@ +// 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/kinematics/SwerveDriveKinematics.h" + +template +struct wpi::Struct> { + static constexpr ct_string kTypeName = wpi::Concat( + "SwerveDriveKinematics__"_ct_string, wpi::NumToCtString()); + static constexpr std::string_view GetTypeName() { return kTypeName; } + static constexpr size_t GetSize() { + return NumModules * wpi::Struct::GetSize(); + } + static constexpr ct_string kSchema = + wpi::Concat("Translation2d modules["_ct_string, + wpi::NumToCtString(), "]"_ct_string); + static constexpr std::string_view GetSchema() { return kSchema; } + + static frc::SwerveDriveKinematics Unpack( + std::span data); + static void Pack(std::span data, + const frc::SwerveDriveKinematics& value); + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + } +}; + +static_assert(wpi::StructSerializable>); +static_assert(wpi::HasNestedStruct>); +static_assert(wpi::StructSerializable>); +static_assert(wpi::HasNestedStruct>); + +#include "frc/kinematics/struct/SwerveDriveKinematicsStruct.inc" diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.inc b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.inc new file mode 100644 index 0000000000..5e4dee13ee --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.inc @@ -0,0 +1,25 @@ +// 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/kinematics/struct/SwerveDriveKinematicsStruct.h" + +template +frc::SwerveDriveKinematics +wpi::Struct>::Unpack( + std::span data) { + constexpr size_t kModulesOff = 0; + return frc::SwerveDriveKinematics{ + wpi::UnpackStructArray( + data)}; +} + +template +void wpi::Struct>::Pack( + std::span data, + const frc::SwerveDriveKinematics& value) { + constexpr size_t kModulesOff = 0; + wpi::PackStructArray(data, value.GetModules()); +} diff --git a/wpimath/src/main/native/include/frc/proto/MatrixProto.h b/wpimath/src/main/native/include/frc/proto/MatrixProto.h new file mode 100644 index 0000000000..1440c513c6 --- /dev/null +++ b/wpimath/src/main/native/include/frc/proto/MatrixProto.h @@ -0,0 +1,22 @@ +// 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 "frc/EigenCore.h" + +template + requires(Cols != 1) +struct wpi::Protobuf> { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Matrixd Unpack( + const google::protobuf::Message& msg); + static void Pack( + google::protobuf::Message* msg, + const frc::Matrixd& value); +}; + +#include "frc/proto/MatrixProto.inc" diff --git a/wpimath/src/main/native/include/frc/proto/MatrixProto.inc b/wpimath/src/main/native/include/frc/proto/MatrixProto.inc new file mode 100644 index 0000000000..75370b6421 --- /dev/null +++ b/wpimath/src/main/native/include/frc/proto/MatrixProto.inc @@ -0,0 +1,60 @@ +// 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/proto/MatrixProto.h" +#include "wpimath.pb.h" + +template + requires(Cols != 1) +google::protobuf::Message* +wpi::Protobuf>::New( + google::protobuf::Arena* arena) { + return wpi::CreateMessage(arena); +} + +template + requires(Cols != 1) +frc::Matrixd +wpi::Protobuf>::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + if (m->num_rows() != Rows || m->num_cols() != Cols) { + throw std::invalid_argument( + fmt::format("Tried to unpack message with {} rows and {} columns into " + "Matrix with {} rows and {} columns", + m->num_rows(), m->num_cols(), Rows, Cols)); + } + if (m->data_size() != Rows * Cols) { + throw std::invalid_argument( + fmt::format("Tried to unpack message with {} elements in data into " + "Matrix with {} elements", + m->data_size(), Rows * Cols)); + } + frc::Matrixd mat; + for (int i = 0; i < Rows * Cols; i++) { + mat(i) = m->data(i); + } + return mat; +} + +template + requires(Cols != 1) +void wpi::Protobuf>::Pack( + google::protobuf::Message* msg, + const frc::Matrixd& value) { + auto m = static_cast(msg); + m->set_num_rows(Rows); + m->set_num_cols(Cols); + m->clear_data(); + for (int i = 0; i < Rows * Cols; i++) { + m->add_data(value(i)); + } +} diff --git a/wpimath/src/main/native/include/frc/proto/VectorProto.h b/wpimath/src/main/native/include/frc/proto/VectorProto.h new file mode 100644 index 0000000000..654f31c088 --- /dev/null +++ b/wpimath/src/main/native/include/frc/proto/VectorProto.h @@ -0,0 +1,21 @@ +// 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 "frc/EigenCore.h" + +template +struct wpi::Protobuf> { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Matrixd Unpack( + const google::protobuf::Message& msg); + static void Pack( + google::protobuf::Message* msg, + const frc::Matrixd& value); +}; + +#include "frc/proto/VectorProto.inc" diff --git a/wpimath/src/main/native/include/frc/proto/VectorProto.inc b/wpimath/src/main/native/include/frc/proto/VectorProto.inc new file mode 100644 index 0000000000..f07d7dfa77 --- /dev/null +++ b/wpimath/src/main/native/include/frc/proto/VectorProto.inc @@ -0,0 +1,49 @@ +// 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/proto/VectorProto.h" +#include "wpimath.pb.h" + +template +google::protobuf::Message* +wpi::Protobuf>::New( + google::protobuf::Arena* arena) { + return wpi::CreateMessage(arena); +} + +template +frc::Matrixd +wpi::Protobuf>::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + if (m->rows_size() != Size) { + throw std::invalid_argument( + fmt::format("Tried to unpack message with {} elements in rows into " + "Vector with {} rows", + m->rows_size(), Size)); + } + frc::Matrixd vec; + for (int i = 0; i < Size; i++) { + vec(i) = m->rows(i); + } + return vec; +} + +template +void wpi::Protobuf>::Pack( + google::protobuf::Message* msg, + const frc::Matrixd& value) { + auto m = static_cast(msg); + m->clear_rows(); + for (int i = 0; i < Size; i++) { + m->add_rows(value(i)); + } +} diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h index 07cc13c981..420566b3d5 100644 --- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h @@ -114,3 +114,6 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> { } }; } // namespace frc + +#include "frc/spline/proto/CubicHermiteSplineProto.h" +#include "frc/spline/struct/CubicHermiteSplineStruct.h" diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h index 37513cc064..fb0d227e29 100644 --- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h @@ -124,3 +124,6 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> { } }; } // namespace frc + +#include "frc/spline/proto/QuinticHermiteSplineProto.h" +#include "frc/spline/struct/QuinticHermiteSplineStruct.h" diff --git a/wpimath/src/main/native/include/frc/spline/proto/CubicHermiteSplineProto.h b/wpimath/src/main/native/include/frc/spline/proto/CubicHermiteSplineProto.h new file mode 100644 index 0000000000..324304b279 --- /dev/null +++ b/wpimath/src/main/native/include/frc/spline/proto/CubicHermiteSplineProto.h @@ -0,0 +1,18 @@ +// 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 "frc/spline/CubicHermiteSpline.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::CubicHermiteSpline Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::CubicHermiteSpline& value); +}; diff --git a/wpimath/src/main/native/include/frc/spline/proto/QuinticHermiteSplineProto.h b/wpimath/src/main/native/include/frc/spline/proto/QuinticHermiteSplineProto.h new file mode 100644 index 0000000000..e84f75476a --- /dev/null +++ b/wpimath/src/main/native/include/frc/spline/proto/QuinticHermiteSplineProto.h @@ -0,0 +1,18 @@ +// 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 "frc/spline/QuinticHermiteSpline.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::QuinticHermiteSpline Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::QuinticHermiteSpline& value); +}; diff --git a/wpimath/src/main/native/include/frc/spline/struct/CubicHermiteSplineStruct.h b/wpimath/src/main/native/include/frc/spline/struct/CubicHermiteSplineStruct.h new file mode 100644 index 0000000000..3ab2557845 --- /dev/null +++ b/wpimath/src/main/native/include/frc/spline/struct/CubicHermiteSplineStruct.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 +#include + +#include "frc/spline/CubicHermiteSpline.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeName() { + return "CubicHermiteSpline"; + } + static constexpr size_t GetSize() { return 4 * 2 * 8; } + static constexpr std::string_view GetSchema() { + return "double xInitial[2];double xFinal[2];double yInitial[2];double " + "yFinal[2]"; + } + + static frc::CubicHermiteSpline Unpack(std::span data); + static void Pack(std::span data, + const frc::CubicHermiteSpline& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpimath/src/main/native/include/frc/spline/struct/QuinticHermiteSplineStruct.h b/wpimath/src/main/native/include/frc/spline/struct/QuinticHermiteSplineStruct.h new file mode 100644 index 0000000000..a0873f6218 --- /dev/null +++ b/wpimath/src/main/native/include/frc/spline/struct/QuinticHermiteSplineStruct.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 +#include + +#include "frc/spline/QuinticHermiteSpline.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeName() { + return "QuinticHermiteSpline"; + } + static constexpr size_t GetSize() { return 4 * 3 * 8; } + static constexpr std::string_view GetSchema() { + return "double xInitial[3];double xFinal[3];double yInitial[3];double " + "yFinal[3]"; + } + + static frc::QuinticHermiteSpline Unpack(std::span data); + static void Pack(std::span data, + const frc::QuinticHermiteSpline& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpimath/src/main/native/include/frc/struct/MatrixStruct.h b/wpimath/src/main/native/include/frc/struct/MatrixStruct.h new file mode 100644 index 0000000000..926109c852 --- /dev/null +++ b/wpimath/src/main/native/include/frc/struct/MatrixStruct.h @@ -0,0 +1,36 @@ +// 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/EigenCore.h" + +template + requires(Cols != 1) +struct wpi::Struct> { + static constexpr ct_string kTypeName = + wpi::Concat("Matrix__"_ct_string, wpi::NumToCtString(), + "_"_ct_string, wpi::NumToCtString()); + static constexpr std::string_view GetTypeName() { return kTypeName; } + static constexpr size_t GetSize() { return Rows * Cols * 8; } + static constexpr ct_string kSchema = + wpi::Concat("double data["_ct_string, wpi::NumToCtString(), + "]"_ct_string); + static constexpr std::string_view GetSchema() { return kSchema; } + + static frc::Matrixd Unpack( + std::span data); + static void Pack( + std::span data, + const frc::Matrixd& value); +}; + +static_assert(wpi::StructSerializable>); +static_assert(wpi::StructSerializable>); + +#include "frc/struct/MatrixStruct.inc" diff --git a/wpimath/src/main/native/include/frc/struct/MatrixStruct.inc b/wpimath/src/main/native/include/frc/struct/MatrixStruct.inc new file mode 100644 index 0000000000..4efda8a41d --- /dev/null +++ b/wpimath/src/main/native/include/frc/struct/MatrixStruct.inc @@ -0,0 +1,35 @@ +// 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/struct/MatrixStruct.h" + +template + requires(Cols != 1) +frc::Matrixd +wpi::Struct>::Unpack( + std::span data) { + constexpr size_t kDataOff = 0; + wpi::array mat_data = + wpi::UnpackStructArray(data); + frc::Matrixd mat; + for (int i = 0; i < Rows * Cols; i++) { + mat(i) = mat_data[i]; + } + return mat; +} + +template + requires(Cols != 1) +void wpi::Struct>::Pack( + std::span data, + const frc::Matrixd& value) { + constexpr size_t kDataOff = 0; + wpi::array mat_data(wpi::empty_array); + for (int i = 0; i < Rows * Cols; i++) { + mat_data[i] = value(i); + } + wpi::PackStructArray(data, mat_data); +} diff --git a/wpimath/src/main/native/include/frc/struct/VectorStruct.h b/wpimath/src/main/native/include/frc/struct/VectorStruct.h new file mode 100644 index 0000000000..d10d48ebe5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/struct/VectorStruct.h @@ -0,0 +1,33 @@ +// 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/EigenCore.h" + +template +struct wpi::Struct> { + static constexpr ct_string kTypeName = + wpi::Concat("Vector__"_ct_string, wpi::NumToCtString()); + static constexpr std::string_view GetTypeName() { return kTypeName; } + static constexpr size_t GetSize() { return Size * 8; } + static constexpr ct_string kSchema = wpi::Concat( + "double data["_ct_string, wpi::NumToCtString(), "]"_ct_string); + static constexpr std::string_view GetSchema() { return kSchema; } + + static frc::Matrixd Unpack( + std::span data); + static void Pack( + std::span data, + const frc::Matrixd& value); +}; + +static_assert(wpi::StructSerializable>); +static_assert(wpi::StructSerializable>); + +#include "frc/struct/VectorStruct.inc" diff --git a/wpimath/src/main/native/include/frc/struct/VectorStruct.inc b/wpimath/src/main/native/include/frc/struct/VectorStruct.inc new file mode 100644 index 0000000000..4fcfbb009d --- /dev/null +++ b/wpimath/src/main/native/include/frc/struct/VectorStruct.inc @@ -0,0 +1,33 @@ +// 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/struct/VectorStruct.h" + +template +frc::Matrixd +wpi::Struct>::Unpack( + std::span data) { + constexpr size_t kDataOff = 0; + wpi::array vec_data = + wpi::UnpackStructArray(data); + frc::Matrixd vec; + for (int i = 0; i < Size; i++) { + vec(i) = vec_data[i]; + } + return vec; +} + +template +void wpi::Struct>::Pack( + std::span data, + const frc::Matrixd& value) { + constexpr size_t kDataOff = 0; + wpi::array vec_data(wpi::empty_array); + for (int i = 0; i < Size; i++) { + vec_data[i] = value(i); + } + wpi::PackStructArray(data, vec_data); +} diff --git a/wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.h b/wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.h new file mode 100644 index 0000000000..6d5452fc72 --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.h @@ -0,0 +1,21 @@ +// 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 "frc/proto/MatrixProto.h" +#include "frc/system/LinearSystem.h" + +template +struct wpi::Protobuf> { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::LinearSystem Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::LinearSystem& value); +}; + +#include "frc/system/proto/LinearSystemProto.inc" diff --git a/wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.inc b/wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.inc new file mode 100644 index 0000000000..f8c0a814b2 --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.inc @@ -0,0 +1,54 @@ +// 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/system/proto/LinearSystemProto.h" +#include "system.pb.h" + +template +google::protobuf::Message* +wpi::Protobuf>::New( + google::protobuf::Arena* arena) { + return wpi::CreateMessage(arena); +} + +template +frc::LinearSystem +wpi::Protobuf>::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + if (m->num_states() != States || m->num_inputs() != Inputs || + m->num_outputs() != Outputs) { + throw std::invalid_argument(fmt::format( + "Tried to unpack message with {} states and {} inputs and {} outputs " + "into LinearSystem with {} states and {} inputs and {} outputs", + m->num_states(), m->num_inputs(), m->num_outputs(), States, Inputs, + Outputs)); + } + return frc::LinearSystem{ + wpi::UnpackProtobuf>(m->a()), + wpi::UnpackProtobuf>(m->b()), + wpi::UnpackProtobuf>(m->c()), + wpi::UnpackProtobuf>(m->d())}; +} + +template +void wpi::Protobuf>::Pack( + google::protobuf::Message* msg, + const frc::LinearSystem& value) { + auto m = static_cast(msg); + m->set_num_states(States); + m->set_num_inputs(Inputs); + m->set_num_outputs(Outputs); + wpi::PackProtobuf(m->mutable_a(), value.A()); + wpi::PackProtobuf(m->mutable_b(), value.B()); + wpi::PackProtobuf(m->mutable_c(), value.C()); + wpi::PackProtobuf(m->mutable_d(), value.D()); +} 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 0000000000..e8731a966b --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.h @@ -0,0 +1,52 @@ +// 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 kTypeName = + wpi::Concat("LinearSystem__"_ct_string, wpi::NumToCtString(), + "_"_ct_string, wpi::NumToCtString(), "_"_ct_string, + wpi::NumToCtString()); + static constexpr std::string_view GetTypeName() { return kTypeName; } + 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 void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema>(fn); + wpi::ForEachStructSchema>(fn); + wpi::ForEachStructSchema>(fn); + wpi::ForEachStructSchema>(fn); + } +}; + +static_assert(wpi::StructSerializable>); +static_assert(wpi::HasNestedStruct>); +static_assert(wpi::StructSerializable>); +static_assert(wpi::HasNestedStruct>); + +#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 0000000000..fbac4b97bc --- /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/main/proto/controller.proto b/wpimath/src/main/proto/controller.proto index 0d0d3fb6ff..dd9db5f311 100644 --- a/wpimath/src/main/proto/controller.proto +++ b/wpimath/src/main/proto/controller.proto @@ -29,6 +29,7 @@ message ProtobufSimpleMotorFeedforward { double ks = 1; double kv = 2; double ka = 3; + double dt = 4; } message ProtobufDifferentialDriveWheelVoltages { diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProtoTest.java new file mode 100644 index 0000000000..52b8adb77b --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProtoTest.java @@ -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. + +package edu.wpi.first.math.controller.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.DifferentialDriveFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveFeedforward; +import edu.wpi.first.wpilibj.ProtoTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class DifferentialDriveFeedforwardProtoTest + extends ProtoTestBase { + DifferentialDriveFeedforwardProtoTest() { + super( + new DifferentialDriveFeedforward(0.174, 0.229, 4.4, 4.5), + DifferentialDriveFeedforward.proto); + } + + @Override + public void checkEquals( + DifferentialDriveFeedforward testData, DifferentialDriveFeedforward data) { + assertEquals(testData.m_kVLinear, data.m_kVLinear); + assertEquals(testData.m_kALinear, data.m_kALinear); + assertEquals(testData.m_kVAngular, data.m_kVAngular); + assertEquals(testData.m_kAAngular, data.m_kAAngular); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProtoTest.java new file mode 100644 index 0000000000..dfe8510da9 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProtoTest.java @@ -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. + +package edu.wpi.first.math.controller.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufSimpleMotorFeedforward; +import edu.wpi.first.wpilibj.ProtoTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class SimpleMotorFeedforwardProtoTest + extends ProtoTestBase { + SimpleMotorFeedforwardProtoTest() { + super(new SimpleMotorFeedforward(0.4, 4.0, 0.7, 0.025), SimpleMotorFeedforward.proto); + } + + @Override + public void checkEquals(SimpleMotorFeedforward testData, SimpleMotorFeedforward data) { + assertEquals(testData.getKs(), data.getKs()); + assertEquals(testData.getKv(), data.getKv()); + assertEquals(testData.getKa(), data.getKa()); + assertEquals(testData.getDt(), data.getDt()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStructTest.java new file mode 100644 index 0000000000..85beea9ce1 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStructTest.java @@ -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. + +package edu.wpi.first.math.controller.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.DifferentialDriveFeedforward; +import edu.wpi.first.wpilibj.StructTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class DifferentialDriveFeedforwardStructTest extends StructTestBase { + DifferentialDriveFeedforwardStructTest() { + super( + new DifferentialDriveFeedforward(0.174, 0.229, 4.4, 4.5), + DifferentialDriveFeedforward.struct); + } + + @Override + public void checkEquals( + DifferentialDriveFeedforward testData, DifferentialDriveFeedforward data) { + assertEquals(testData.m_kVLinear, data.m_kVLinear); + assertEquals(testData.m_kALinear, data.m_kALinear); + assertEquals(testData.m_kVAngular, data.m_kVAngular); + assertEquals(testData.m_kAAngular, data.m_kAAngular); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStructTest.java new file mode 100644 index 0000000000..9dba1295e2 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStructTest.java @@ -0,0 +1,25 @@ +// 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.controller.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.StructTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class SimpleMotorFeedforwardStructTest extends StructTestBase { + SimpleMotorFeedforwardStructTest() { + super(new SimpleMotorFeedforward(0.4, 4.0, 0.7, 0.025), SimpleMotorFeedforward.struct); + } + + @Override + public void checkEquals(SimpleMotorFeedforward testData, SimpleMotorFeedforward data) { + assertEquals(testData.getKs(), data.getKs()); + assertEquals(testData.getKv(), data.getKv()); + assertEquals(testData.getKa(), data.getKa()); + assertEquals(testData.getDt(), data.getDt()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProtoTest.java new file mode 100644 index 0000000000..0aaac653f4 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProtoTest.java @@ -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. + +package edu.wpi.first.math.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveMotorVoltages; +import edu.wpi.first.wpilibj.ProtoTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class MecanumDriveMotorVoltagesProtoTest + extends ProtoTestBase { + MecanumDriveMotorVoltagesProtoTest() { + super(new MecanumDriveMotorVoltages(1.2, 3.1, 2.5, -0.1), MecanumDriveMotorVoltages.proto); + } + + @Override + public void checkEquals(MecanumDriveMotorVoltages testData, MecanumDriveMotorVoltages data) { + assertEquals(testData.frontLeftVoltage, data.frontLeftVoltage); + assertEquals(testData.frontRightVoltage, data.frontRightVoltage); + assertEquals(testData.rearLeftVoltage, data.rearLeftVoltage); + assertEquals(testData.rearRightVoltage, data.rearRightVoltage); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProtoTest.java new file mode 100644 index 0000000000..8304d467ea --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProtoTest.java @@ -0,0 +1,31 @@ +// 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 static org.junit.jupiter.api.Assertions.assertArrayEquals; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveDriveKinematics; +import edu.wpi.first.wpilibj.ProtoTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class SwerveDriveKinematicsProtoTest + extends ProtoTestBase { + SwerveDriveKinematicsProtoTest() { + super( + new SwerveDriveKinematics( + new Translation2d(1.0, 2.1), + new Translation2d(1.5, -0.9), + new Translation2d(-1.8, 1.2), + new Translation2d(-1.7, -1.3)), + SwerveDriveKinematics.proto); + } + + @Override + public void checkEquals(SwerveDriveKinematics testData, SwerveDriveKinematics data) { + assertArrayEquals(testData.getModules(), data.getModules()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStructTest.java new file mode 100644 index 0000000000..a53fbbadd4 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStructTest.java @@ -0,0 +1,25 @@ +// 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 static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages; +import edu.wpi.first.wpilibj.StructTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class MecanumDriveMotorVoltagesStructTest extends StructTestBase { + MecanumDriveMotorVoltagesStructTest() { + super(new MecanumDriveMotorVoltages(1.2, 3.1, 2.5, -0.1), MecanumDriveMotorVoltages.struct); + } + + @Override + public void checkEquals(MecanumDriveMotorVoltages testData, MecanumDriveMotorVoltages data) { + assertEquals(testData.frontLeftVoltage, data.frontLeftVoltage); + assertEquals(testData.frontRightVoltage, data.frontRightVoltage); + assertEquals(testData.rearLeftVoltage, data.rearLeftVoltage); + assertEquals(testData.rearRightVoltage, data.rearRightVoltage); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStructTest.java new file mode 100644 index 0000000000..1e7c335fdb --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStructTest.java @@ -0,0 +1,29 @@ +// 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 static org.junit.jupiter.api.Assertions.assertArrayEquals; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj.StructTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class SwerveDriveKinematicsStructTest extends StructTestBase { + SwerveDriveKinematicsStructTest() { + super( + new SwerveDriveKinematics( + new Translation2d(1.0, 2.1), + new Translation2d(1.5, -0.9), + new Translation2d(-1.8, 1.2), + new Translation2d(-1.7, -1.3)), + SwerveDriveKinematics.getStruct(4)); + } + + @Override + public void checkEquals(SwerveDriveKinematics testData, SwerveDriveKinematics data) { + assertArrayEquals(testData.getModules(), data.getModules()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/proto/MatrixProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/proto/MatrixProtoTest.java new file mode 100644 index 0000000000..64c37d5bbf --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/proto/MatrixProtoTest.java @@ -0,0 +1,29 @@ +// 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.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +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.proto.Wpimath.ProtobufMatrix; +import edu.wpi.first.wpilibj.ProtoTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class MatrixProtoTest extends ProtoTestBase, ProtobufMatrix> { + MatrixProtoTest() { + super( + MatBuilder.fill(Nat.N2(), Nat.N3(), 1.1, 1.2, 1.3, 1.4, 1.5, 1.6), + Matrix.getProto(Nat.N2(), Nat.N3())); + } + + @Override + public void checkEquals(Matrix testData, Matrix data) { + assertEquals(testData, data); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/proto/VectorProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/proto/VectorProtoTest.java new file mode 100644 index 0000000000..d3be6b62f5 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/proto/VectorProtoTest.java @@ -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. + +package edu.wpi.first.math.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.proto.Wpimath.ProtobufVector; +import edu.wpi.first.wpilibj.ProtoTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class VectorProtoTest extends ProtoTestBase, ProtobufVector> { + VectorProtoTest() { + super(VecBuilder.fill(1.1, 1.2), Vector.getProto(Nat.N2())); + } + + @Override + public void checkEquals(Vector testData, Vector data) { + assertEquals(testData, data); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/proto/CubicHermiteSplineProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/proto/CubicHermiteSplineProtoTest.java new file mode 100644 index 0000000000..36f6567349 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/spline/proto/CubicHermiteSplineProtoTest.java @@ -0,0 +1,33 @@ +// 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.spline.proto; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; + +import edu.wpi.first.math.proto.Spline.ProtobufCubicHermiteSpline; +import edu.wpi.first.math.spline.CubicHermiteSpline; +import edu.wpi.first.wpilibj.ProtoTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class CubicHermiteSplineProtoTest + extends ProtoTestBase { + CubicHermiteSplineProtoTest() { + super( + new CubicHermiteSpline( + new double[] {0.1, 0.3}, + new double[] {0.4, -0.2}, + new double[] {1.5, 1.3}, + new double[] {-2.4, -1.1}), + CubicHermiteSpline.proto); + } + + @Override + public void checkEquals(CubicHermiteSpline testData, CubicHermiteSpline data) { + assertArrayEquals(testData.xInitialControlVector, data.xInitialControlVector); + assertArrayEquals(testData.xFinalControlVector, data.xFinalControlVector); + assertArrayEquals(testData.yInitialControlVector, data.yInitialControlVector); + assertArrayEquals(testData.yFinalControlVector, data.yFinalControlVector); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/proto/QuinticHermiteSplineProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/proto/QuinticHermiteSplineProtoTest.java new file mode 100644 index 0000000000..6254bbb304 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/spline/proto/QuinticHermiteSplineProtoTest.java @@ -0,0 +1,33 @@ +// 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.spline.proto; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; + +import edu.wpi.first.math.proto.Spline.ProtobufQuinticHermiteSpline; +import edu.wpi.first.math.spline.QuinticHermiteSpline; +import edu.wpi.first.wpilibj.ProtoTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class QuinticHermiteSplineProtoTest + extends ProtoTestBase { + QuinticHermiteSplineProtoTest() { + super( + new QuinticHermiteSpline( + new double[] {0.1, 0.3, 0.7}, + new double[] {0.4, -0.2, -0.6}, + new double[] {1.5, 1.3, 1.6}, + new double[] {-2.4, -1.1, -2.1}), + QuinticHermiteSpline.proto); + } + + @Override + public void checkEquals(QuinticHermiteSpline testData, QuinticHermiteSpline data) { + assertArrayEquals(testData.xInitialControlVector, data.xInitialControlVector); + assertArrayEquals(testData.xFinalControlVector, data.xFinalControlVector); + assertArrayEquals(testData.yInitialControlVector, data.yInitialControlVector); + assertArrayEquals(testData.yFinalControlVector, data.yFinalControlVector); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/struct/CubicHermiteSplineStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/struct/CubicHermiteSplineStructTest.java new file mode 100644 index 0000000000..948bc4c03a --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/spline/struct/CubicHermiteSplineStructTest.java @@ -0,0 +1,31 @@ +// 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.spline.struct; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; + +import edu.wpi.first.math.spline.CubicHermiteSpline; +import edu.wpi.first.wpilibj.StructTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class CubicHermiteSplineStructTest extends StructTestBase { + CubicHermiteSplineStructTest() { + super( + new CubicHermiteSpline( + new double[] {0.1, 0.3}, + new double[] {0.4, -0.2}, + new double[] {1.5, 1.3}, + new double[] {-2.4, -1.1}), + CubicHermiteSpline.struct); + } + + @Override + public void checkEquals(CubicHermiteSpline testData, CubicHermiteSpline data) { + assertArrayEquals(testData.xInitialControlVector, data.xInitialControlVector); + assertArrayEquals(testData.xFinalControlVector, data.xFinalControlVector); + assertArrayEquals(testData.yInitialControlVector, data.yInitialControlVector); + assertArrayEquals(testData.yFinalControlVector, data.yFinalControlVector); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/struct/QuinticHermiteSplineStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/struct/QuinticHermiteSplineStructTest.java new file mode 100644 index 0000000000..9305661746 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/spline/struct/QuinticHermiteSplineStructTest.java @@ -0,0 +1,31 @@ +// 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.spline.struct; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; + +import edu.wpi.first.math.spline.QuinticHermiteSpline; +import edu.wpi.first.wpilibj.StructTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class QuinticHermiteSplineStructTest extends StructTestBase { + QuinticHermiteSplineStructTest() { + super( + new QuinticHermiteSpline( + new double[] {0.1, 0.3, 0.7}, + new double[] {0.4, -0.2, -0.6}, + new double[] {1.5, 1.3, 1.6}, + new double[] {-2.4, -1.1, -2.1}), + QuinticHermiteSpline.struct); + } + + @Override + public void checkEquals(QuinticHermiteSpline testData, QuinticHermiteSpline data) { + assertArrayEquals(testData.xInitialControlVector, data.xInitialControlVector); + assertArrayEquals(testData.xFinalControlVector, data.xFinalControlVector); + assertArrayEquals(testData.yInitialControlVector, data.yInitialControlVector); + assertArrayEquals(testData.yFinalControlVector, data.yFinalControlVector); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/struct/MatrixStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/struct/MatrixStructTest.java new file mode 100644 index 0000000000..6a9e9bb58c --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/struct/MatrixStructTest.java @@ -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. + +package edu.wpi.first.math.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.StructTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class MatrixStructTest extends StructTestBase> { + MatrixStructTest() { + super( + MatBuilder.fill(Nat.N2(), Nat.N3(), 1.1, 1.2, 1.3, 1.4, 1.5, 1.6), + Matrix.getStruct(Nat.N2(), Nat.N3())); + } + + @Override + public void checkEquals(Matrix testData, Matrix data) { + assertEquals(testData, data); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/struct/VectorStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/struct/VectorStructTest.java new file mode 100644 index 0000000000..9685945718 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/struct/VectorStructTest.java @@ -0,0 +1,25 @@ +// 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.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.wpilibj.StructTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class VectorStructTest extends StructTestBase> { + VectorStructTest() { + super(VecBuilder.fill(1.1, 1.2), Vector.getStruct(Nat.N2())); + } + + @Override + public void checkEquals(Vector testData, Vector data) { + assertEquals(testData, data); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/proto/LinearSystemProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/proto/LinearSystemProtoTest.java new file mode 100644 index 0000000000..3352f3c0d9 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/system/proto/LinearSystemProtoTest.java @@ -0,0 +1,39 @@ +// 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 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.proto.System.ProtobufLinearSystem; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.wpilibj.ProtoTestBase; + +@SuppressWarnings("PMD.TestClassWithoutTestCases") +class LinearSystemProtoTest extends ProtoTestBase, ProtobufLinearSystem> { + LinearSystemProtoTest() { + 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.getProto(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/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 0000000000..4dc56dabc9 --- /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/java/edu/wpi/first/wpilibj/ProtoTestBase.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/ProtoTestBase.java new file mode 100644 index 0000000000..36bbb6d651 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/ProtoTestBase.java @@ -0,0 +1,52 @@ +// 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.wpilibj; + +import edu.wpi.first.util.protobuf.Protobuf; +import org.junit.jupiter.api.Test; +import us.hebi.quickbuf.ProtoMessage; + +public abstract class ProtoTestBase> { + private final T m_testData; + private final Protobuf m_proto; + + protected ProtoTestBase(T testData, Protobuf proto) { + m_testData = testData; + m_proto = proto; + } + + public abstract void checkEquals(T testData, T data); + + @Test + void testRoundTrip() { + final MessageType msg = m_proto.createMessage(); + m_proto.pack(msg, m_testData); + + final T data = m_proto.unpack(msg); + checkEquals(m_testData, data); + } + + @Test + void testDoublePack() { + final MessageType msg = m_proto.createMessage(); + m_proto.pack(msg, m_testData); + m_proto.pack(msg, m_testData); + + final T data = m_proto.unpack(msg); + checkEquals(m_testData, data); + } + + @Test + void testDoubleUnpack() { + final MessageType msg = m_proto.createMessage(); + m_proto.pack(msg, m_testData); + + T data = m_proto.unpack(msg); + checkEquals(m_testData, data); + + data = m_proto.unpack(msg); + checkEquals(m_testData, data); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/StructTestBase.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/StructTestBase.java new file mode 100644 index 0000000000..16c6093c38 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/StructTestBase.java @@ -0,0 +1,61 @@ +// 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.wpilibj; + +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +public abstract class StructTestBase { + private final T m_testData; + private final Struct m_struct; + + protected StructTestBase(T testData, Struct struct) { + m_testData = testData; + m_struct = struct; + } + + public abstract void checkEquals(T testData, T data); + + @Test + void testRoundTrip() { + ByteBuffer buffer = ByteBuffer.allocate(m_struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + m_struct.pack(buffer, m_testData); + buffer.rewind(); + + final T data = m_struct.unpack(buffer); + checkEquals(m_testData, data); + } + + @Test + void testDoublePack() { + ByteBuffer buffer = ByteBuffer.allocate(m_struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + m_struct.pack(buffer, m_testData); + buffer.rewind(); + m_struct.pack(buffer, m_testData); + buffer.rewind(); + + final T data = m_struct.unpack(buffer); + checkEquals(m_testData, data); + } + + @Test + void testDoubleUnpack() { + ByteBuffer buffer = ByteBuffer.allocate(m_struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + m_struct.pack(buffer, m_testData); + buffer.rewind(); + + T data = m_struct.unpack(buffer); + checkEquals(m_testData, data); + buffer.rewind(); + + data = m_struct.unpack(buffer); + checkEquals(m_testData, data); + } +} diff --git a/wpimath/src/test/native/cpp/ProtoTestBase.h b/wpimath/src/test/native/cpp/ProtoTestBase.h new file mode 100644 index 0000000000..88e149e3e0 --- /dev/null +++ b/wpimath/src/test/native/cpp/ProtoTestBase.h @@ -0,0 +1,59 @@ +// 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 "controller.pb.h" +#include "kinematics.pb.h" +#include "spline.pb.h" +#include "system.pb.h" +#include "wpimath.pb.h" + +template +class ProtoTest : public testing::Test {}; + +TYPED_TEST_SUITE_P(ProtoTest); + +TYPED_TEST_P(ProtoTest, RoundTrip) { + using Type = typename TypeParam::Type; + google::protobuf::Arena arena; + google::protobuf::Message* proto = wpi::Protobuf::New(&arena); + wpi::PackProtobuf(proto, TypeParam::kTestData); + + Type unpacked_data = wpi::UnpackProtobuf(*proto); + TypeParam::CheckEq(TypeParam::kTestData, unpacked_data); +} + +TYPED_TEST_P(ProtoTest, DoublePack) { + using Type = typename TypeParam::Type; + google::protobuf::Arena arena; + google::protobuf::Message* proto = wpi::Protobuf::New(&arena); + wpi::PackProtobuf(proto, TypeParam::kTestData); + wpi::PackProtobuf(proto, TypeParam::kTestData); + + Type unpacked_data = wpi::UnpackProtobuf(*proto); + TypeParam::CheckEq(TypeParam::kTestData, unpacked_data); +} + +TYPED_TEST_P(ProtoTest, DoubleUnpack) { + using Type = typename TypeParam::Type; + google::protobuf::Arena arena; + google::protobuf::Message* proto = wpi::Protobuf::New(&arena); + wpi::PackProtobuf(proto, TypeParam::kTestData); + + { + Type unpacked_data = wpi::UnpackProtobuf(*proto); + TypeParam::CheckEq(TypeParam::kTestData, unpacked_data); + } + + { + Type unpacked_data = wpi::UnpackProtobuf(*proto); + TypeParam::CheckEq(TypeParam::kTestData, unpacked_data); + } +} + +REGISTER_TYPED_TEST_SUITE_P(ProtoTest, RoundTrip, DoublePack, DoubleUnpack); diff --git a/wpimath/src/test/native/cpp/StructTestBase.h b/wpimath/src/test/native/cpp/StructTestBase.h new file mode 100644 index 0000000000..ef120ec9a8 --- /dev/null +++ b/wpimath/src/test/native/cpp/StructTestBase.h @@ -0,0 +1,61 @@ +// 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 + +template +class StructTest : public testing::Test {}; + +TYPED_TEST_SUITE_P(StructTest); + +// For these tests: +// TypeParam defines Type, kTestData, and CheckEq +// Type is the data type +// StructType is the instantiation of wpi::Struct<> + +TYPED_TEST_P(StructTest, RoundTrip) { + using Type = typename TypeParam::Type; + using StructType = wpi::Struct; + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); + wpi::PackStruct(buffer, TypeParam::kTestData); + + Type unpacked_data = wpi::UnpackStruct(buffer); + TypeParam::CheckEq(TypeParam::kTestData, unpacked_data); +} + +TYPED_TEST_P(StructTest, DoublePack) { + using Type = typename TypeParam::Type; + using StructType = wpi::Struct; + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); + wpi::PackStruct(buffer, TypeParam::kTestData); + wpi::PackStruct(buffer, TypeParam::kTestData); + + Type unpacked_data = wpi::UnpackStruct(buffer); + TypeParam::CheckEq(TypeParam::kTestData, unpacked_data); +} + +TYPED_TEST_P(StructTest, DoubleUnpack) { + using Type = typename TypeParam::Type; + using StructType = wpi::Struct; + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); + wpi::PackStruct(buffer, TypeParam::kTestData); + + { + Type unpacked_data = wpi::UnpackStruct(buffer); + TypeParam::CheckEq(TypeParam::kTestData, unpacked_data); + } + + { + Type unpacked_data = wpi::UnpackStruct(buffer); + TypeParam::CheckEq(TypeParam::kTestData, unpacked_data); + } +} + +REGISTER_TYPED_TEST_SUITE_P(StructTest, RoundTrip, DoublePack, DoubleUnpack); diff --git a/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveFeedforwardProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveFeedforwardProtoTest.cpp new file mode 100644 index 0000000000..e711f97db2 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveFeedforwardProtoTest.cpp @@ -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. + +#include + +#include "../../ProtoTestBase.h" +#include "frc/controller/DifferentialDriveFeedforward.h" + +using namespace frc; + +struct DifferentialDriveFeedforwardProtoTestData { + using Type = DifferentialDriveFeedforward; + + inline static const Type kTestData{ + decltype(1_V / 1_mps){0.174}, decltype(1_V / 1_mps_sq){0.229}, + decltype(1_V / 1_mps){4.4}, decltype(1_V / 1_mps_sq){4.5}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.m_kVLinear.value(), data.m_kVLinear.value()); + EXPECT_EQ(testData.m_kALinear.value(), data.m_kALinear.value()); + EXPECT_EQ(testData.m_kVAngular.value(), data.m_kVAngular.value()); + EXPECT_EQ(testData.m_kAAngular.value(), data.m_kAAngular.value()); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(DifferentialDriveFeedforward, ProtoTest, + DifferentialDriveFeedforwardProtoTestData); diff --git a/wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp new file mode 100644 index 0000000000..e49bafc652 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp @@ -0,0 +1,31 @@ +// 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 "../../ProtoTestBase.h" +#include "frc/controller/SimpleMotorFeedforward.h" +#include "frc/controller/proto/SimpleMotorFeedforwardProto.h" +#include "units/acceleration.h" +#include "units/velocity.h" + +using namespace frc; + +struct SimpleMotorFeedforwardProtoTestData { + using Type = SimpleMotorFeedforward; + + inline static const Type kTestData = {units::volt_t{0.4}, + units::volt_t{4.0} / 1_mps, + units::volt_t{0.7} / 1_mps_sq, 25_ms}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.GetKs().value(), data.GetKs().value()); + EXPECT_EQ(testData.GetKv().value(), data.GetKv().value()); + EXPECT_EQ(testData.GetKa().value(), data.GetKa().value()); + EXPECT_EQ(testData.GetDt().value(), data.GetDt().value()); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(SimpleMotorFeedforwardMeters, ProtoTest, + SimpleMotorFeedforwardProtoTestData); diff --git a/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveFeedforwardStructTest.cpp b/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveFeedforwardStructTest.cpp new file mode 100644 index 0000000000..14c0b3fbb0 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveFeedforwardStructTest.cpp @@ -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. + +#include + +#include "../../StructTestBase.h" +#include "frc/controller/DifferentialDriveFeedforward.h" + +using namespace frc; + +struct DifferentialDriveFeedforwardStructTestData { + using Type = DifferentialDriveFeedforward; + + inline static const Type kTestData{ + decltype(1_V / 1_mps){0.174}, decltype(1_V / 1_mps_sq){0.229}, + decltype(1_V / 1_mps){4.4}, decltype(1_V / 1_mps_sq){4.5}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.m_kVLinear.value(), data.m_kVLinear.value()); + EXPECT_EQ(testData.m_kALinear.value(), data.m_kALinear.value()); + EXPECT_EQ(testData.m_kVAngular.value(), data.m_kVAngular.value()); + EXPECT_EQ(testData.m_kAAngular.value(), data.m_kAAngular.value()); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(DifferentialDriveFeedforward, StructTest, + DifferentialDriveFeedforwardStructTestData); diff --git a/wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp b/wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp new file mode 100644 index 0000000000..ca88fdaa47 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp @@ -0,0 +1,31 @@ +// 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/controller/SimpleMotorFeedforward.h" +#include "frc/controller/struct/SimpleMotorFeedforwardStruct.h" +#include "units/acceleration.h" +#include "units/velocity.h" + +using namespace frc; + +struct SimpleMotorFeedforwardStructTestData { + using Type = SimpleMotorFeedforward; + + inline static const Type kTestData = {units::volt_t{0.4}, + units::volt_t{4.0} / 1_mps, + units::volt_t{0.7} / 1_mps_sq, 25_ms}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.GetKs().value(), data.GetKs().value()); + EXPECT_EQ(testData.GetKv().value(), data.GetKv().value()); + EXPECT_EQ(testData.GetKa().value(), data.GetKa().value()); + EXPECT_EQ(testData.GetDt().value(), data.GetDt().value()); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(SimpleMotorFeedforwardMeters, StructTest, + SimpleMotorFeedforwardStructTestData); diff --git a/wpimath/src/test/native/cpp/kinematics/proto/SwerveDriveKinematicsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/SwerveDriveKinematicsProtoTest.cpp new file mode 100644 index 0000000000..80af48935f --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/SwerveDriveKinematicsProtoTest.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 + +#include "../../ProtoTestBase.h" +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/kinematics/proto/SwerveDriveKinematicsProto.h" + +using namespace frc; + +struct SwerveDriveKinematicsProtoTestData { + using Type = SwerveDriveKinematics<4>; + + inline static const Type kTestData{ + frc::Translation2d{1.0_m, 0.9_m}, frc::Translation2d{1.1_m, -0.8_m}, + frc::Translation2d{-1.2_m, 0.7_m}, frc::Translation2d{-1.3_m, -0.6_m}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.GetModules(), data.GetModules()); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(SwerveDriveKinematics, ProtoTest, + SwerveDriveKinematicsProtoTestData); diff --git a/wpimath/src/test/native/cpp/kinematics/struct/SwerveDriveKinematicsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/SwerveDriveKinematicsStructTest.cpp new file mode 100644 index 0000000000..f05a8ae387 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/SwerveDriveKinematicsStructTest.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 + +#include "../../StructTestBase.h" +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/kinematics/struct/SwerveDriveKinematicsStruct.h" + +using namespace frc; + +struct SwerveDriveKinematicsStructTestData { + using Type = SwerveDriveKinematics<4>; + + inline static const Type kTestData{ + frc::Translation2d{1.0_m, 0.9_m}, frc::Translation2d{1.1_m, -0.8_m}, + frc::Translation2d{-1.2_m, 0.7_m}, frc::Translation2d{-1.3_m, -0.6_m}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.GetModules(), data.GetModules()); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(SwerveDriveKinematics, StructTest, + SwerveDriveKinematicsStructTestData); diff --git a/wpimath/src/test/native/cpp/proto/MatrixProtoTest.cpp b/wpimath/src/test/native/cpp/proto/MatrixProtoTest.cpp new file mode 100644 index 0000000000..0ef91f7420 --- /dev/null +++ b/wpimath/src/test/native/cpp/proto/MatrixProtoTest.cpp @@ -0,0 +1,23 @@ +// 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 "../ProtoTestBase.h" +#include "frc/EigenCore.h" +#include "frc/proto/MatrixProto.h" + +using namespace frc; + +struct MatrixProtoTestData { + using Type = Matrixd<2, 3>; + + inline static const Type kTestData{{1.1, 1.2, 1.3}, {1.4, 1.5, 1.6}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData, data); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(Matrix, ProtoTest, MatrixProtoTestData); diff --git a/wpimath/src/test/native/cpp/proto/VectorProtoTest.cpp b/wpimath/src/test/native/cpp/proto/VectorProtoTest.cpp new file mode 100644 index 0000000000..8ea244e6ac --- /dev/null +++ b/wpimath/src/test/native/cpp/proto/VectorProtoTest.cpp @@ -0,0 +1,23 @@ +// 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 "../ProtoTestBase.h" +#include "frc/EigenCore.h" +#include "frc/proto/VectorProto.h" + +using namespace frc; + +struct VectorProtoTestData { + using Type = Vectord<2>; + + inline static const Type kTestData{1.1, 1.2}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData, data); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(Vector, ProtoTest, VectorProtoTestData); diff --git a/wpimath/src/test/native/cpp/spline/proto/CubicHermiteSplineProtoTest.cpp b/wpimath/src/test/native/cpp/spline/proto/CubicHermiteSplineProtoTest.cpp new file mode 100644 index 0000000000..4e9439858a --- /dev/null +++ b/wpimath/src/test/native/cpp/spline/proto/CubicHermiteSplineProtoTest.cpp @@ -0,0 +1,32 @@ +// 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 "../../ProtoTestBase.h" +#include "frc/spline/CubicHermiteSpline.h" + +using namespace frc; + +struct CubicHermiteSplineProtoTestData { + using Type = CubicHermiteSpline; + + inline static const Type kTestData{ + wpi::array{{0.1, 0.2}}, wpi::array{{0.3, 0.4}}, + wpi::array{{0.5, 0.6}}, wpi::array{{0.7, 0.8}}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.GetInitialControlVector().x, + data.GetInitialControlVector().x); + EXPECT_EQ(testData.GetFinalControlVector().x, + data.GetFinalControlVector().x); + EXPECT_EQ(testData.GetInitialControlVector().y, + data.GetInitialControlVector().y); + EXPECT_EQ(testData.GetFinalControlVector().y, + data.GetFinalControlVector().y); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(CubicHermiteSpline, ProtoTest, + CubicHermiteSplineProtoTestData); diff --git a/wpimath/src/test/native/cpp/spline/proto/QuinticHermiteSplineProtoTest.cpp b/wpimath/src/test/native/cpp/spline/proto/QuinticHermiteSplineProtoTest.cpp new file mode 100644 index 0000000000..13c83e82dc --- /dev/null +++ b/wpimath/src/test/native/cpp/spline/proto/QuinticHermiteSplineProtoTest.cpp @@ -0,0 +1,33 @@ +// 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 "../../ProtoTestBase.h" +#include "frc/spline/QuinticHermiteSpline.h" + +using namespace frc; + +struct QuinticHermiteSplineProtoTestData { + using Type = QuinticHermiteSpline; + + inline static const Type kTestData{wpi::array{{0.01, 0.02, 0.03}}, + wpi::array{{0.04, 0.05, 0.06}}, + wpi::array{{0.07, 0.08, 0.09}}, + wpi::array{{0.10, 0.11, 0.11}}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.GetInitialControlVector().x, + data.GetInitialControlVector().x); + EXPECT_EQ(testData.GetFinalControlVector().x, + data.GetFinalControlVector().x); + EXPECT_EQ(testData.GetInitialControlVector().y, + data.GetInitialControlVector().y); + EXPECT_EQ(testData.GetFinalControlVector().y, + data.GetFinalControlVector().y); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(QuinticHermiteSpline, ProtoTest, + QuinticHermiteSplineProtoTestData); diff --git a/wpimath/src/test/native/cpp/spline/struct/CubicHermiteSplineStructTest.cpp b/wpimath/src/test/native/cpp/spline/struct/CubicHermiteSplineStructTest.cpp new file mode 100644 index 0000000000..fce52ee3c3 --- /dev/null +++ b/wpimath/src/test/native/cpp/spline/struct/CubicHermiteSplineStructTest.cpp @@ -0,0 +1,32 @@ +// 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/spline/CubicHermiteSpline.h" + +using namespace frc; + +struct CubicHermiteSplineStructTestData { + using Type = CubicHermiteSpline; + + inline static const Type kTestData{ + wpi::array{{0.1, 0.2}}, wpi::array{{0.3, 0.4}}, + wpi::array{{0.5, 0.6}}, wpi::array{{0.7, 0.8}}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.GetInitialControlVector().x, + data.GetInitialControlVector().x); + EXPECT_EQ(testData.GetFinalControlVector().x, + data.GetFinalControlVector().x); + EXPECT_EQ(testData.GetInitialControlVector().y, + data.GetInitialControlVector().y); + EXPECT_EQ(testData.GetFinalControlVector().y, + data.GetFinalControlVector().y); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(CubicHermiteSpline, StructTest, + CubicHermiteSplineStructTestData); diff --git a/wpimath/src/test/native/cpp/spline/struct/QuinticHermiteSplineStructTest.cpp b/wpimath/src/test/native/cpp/spline/struct/QuinticHermiteSplineStructTest.cpp new file mode 100644 index 0000000000..33f3380abf --- /dev/null +++ b/wpimath/src/test/native/cpp/spline/struct/QuinticHermiteSplineStructTest.cpp @@ -0,0 +1,33 @@ +// 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/spline/QuinticHermiteSpline.h" + +using namespace frc; + +struct QuinticHermiteSplineStructTestData { + using Type = QuinticHermiteSpline; + + inline static const Type kTestData{wpi::array{{0.01, 0.02, 0.03}}, + wpi::array{{0.04, 0.05, 0.06}}, + wpi::array{{0.07, 0.08, 0.09}}, + wpi::array{{0.10, 0.11, 0.11}}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData.GetInitialControlVector().x, + data.GetInitialControlVector().x); + EXPECT_EQ(testData.GetFinalControlVector().x, + data.GetFinalControlVector().x); + EXPECT_EQ(testData.GetInitialControlVector().y, + data.GetInitialControlVector().y); + EXPECT_EQ(testData.GetFinalControlVector().y, + data.GetFinalControlVector().y); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(QuinticHermiteSpline, StructTest, + QuinticHermiteSplineStructTestData); diff --git a/wpimath/src/test/native/cpp/struct/MatrixStructTest.cpp b/wpimath/src/test/native/cpp/struct/MatrixStructTest.cpp new file mode 100644 index 0000000000..5445b3b0d1 --- /dev/null +++ b/wpimath/src/test/native/cpp/struct/MatrixStructTest.cpp @@ -0,0 +1,23 @@ +// 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/EigenCore.h" +#include "frc/struct/MatrixStruct.h" + +using namespace frc; + +struct MatrixStructTestData { + using Type = Matrixd<2, 3>; + + inline static const Type kTestData{{1.1, 1.2, 1.3}, {1.4, 1.5, 1.6}}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData, data); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(Matrix, StructTest, MatrixStructTestData); diff --git a/wpimath/src/test/native/cpp/struct/VectorStructTest.cpp b/wpimath/src/test/native/cpp/struct/VectorStructTest.cpp new file mode 100644 index 0000000000..cae75d8a56 --- /dev/null +++ b/wpimath/src/test/native/cpp/struct/VectorStructTest.cpp @@ -0,0 +1,23 @@ +// 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/EigenCore.h" +#include "frc/struct/VectorStruct.h" + +using namespace frc; + +struct VectorStructTestData { + using Type = Vectord<2>; + + inline static const Type kTestData{1.1, 1.2}; + + static void CheckEq(const Type& testData, const Type& data) { + EXPECT_EQ(testData, data); + } +}; + +INSTANTIATE_TYPED_TEST_SUITE_P(Vector, StructTest, VectorStructTestData); diff --git a/wpimath/src/test/native/cpp/system/proto/LinearSystemProtoTest.cpp b/wpimath/src/test/native/cpp/system/proto/LinearSystemProtoTest.cpp new file mode 100644 index 0000000000..7a504050f1 --- /dev/null +++ b/wpimath/src/test/native/cpp/system/proto/LinearSystemProtoTest.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 "../../ProtoTestBase.h" +#include "frc/system/LinearSystem.h" +#include "frc/system/proto/LinearSystemProto.h" + +using namespace frc; + +struct LinearSystemProtoTestData { + 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, ProtoTest, + LinearSystemProtoTestData); 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 0000000000..b1d01b01ec --- /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); diff --git a/wpiutil/src/main/java/edu/wpi/first/util/protobuf/Protobuf.java b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/Protobuf.java index f414839900..e834d8e324 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/protobuf/Protobuf.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/Protobuf.java @@ -4,11 +4,14 @@ package edu.wpi.first.util.protobuf; +import java.lang.reflect.Array; import java.util.function.BiConsumer; import java.util.function.Predicate; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.Descriptors.FileDescriptor; import us.hebi.quickbuf.ProtoMessage; +import us.hebi.quickbuf.RepeatedDouble; +import us.hebi.quickbuf.RepeatedMessage; /** * Interface for Protobuf serialization. @@ -18,7 +21,8 @@ import us.hebi.quickbuf.ProtoMessage; * code is auto-generated from .proto interface descriptions (the MessageType generic parameter). * *

Idiomatically, classes that support protobuf serialization should provide a static final - * member named "proto" that provides an instance of an implementation of this interface. + * member named "proto" that provides an instance of an implementation of this interface, or a + * static final method named "getProto" if the class is generic. * * @param object type * @param protobuf message type @@ -142,4 +146,67 @@ public interface Protobuf> { } fn.accept(name, desc.toProtoBytes()); } + + /** + * Unpack a serialized protobuf array message. + * + * @param object type + * @param element type of the protobuf array + * @param msg protobuf array message + * @param proto protobuf implementation + * @return Deserialized array + */ + static > T[] unpackArray( + RepeatedMessage msg, Protobuf proto) { + @SuppressWarnings("unchecked") + T[] result = (T[]) Array.newInstance(proto.getTypeClass(), msg.length()); + for (int i = 0; i < result.length; i++) { + result[i] = proto.unpack(msg.get(i)); + } + return result; + } + + /** + * Unpack a serialized protobuf double array message. + * + * @param msg protobuf double array message + * @return Deserialized array + */ + static double[] unpackArray(RepeatedDouble msg) { + double[] result = new double[msg.length()]; + for (int i = 0; i < result.length; i++) { + result[i] = msg.get(i); + } + return result; + } + + /** + * Pack a serialized protobuf array message. + * + * @param object type + * @param element type of the protobuf array + * @param msg protobuf array message + * @param arr array of objects + * @param proto protobuf implementation + */ + static > void packArray( + RepeatedMessage msg, T[] arr, Protobuf proto) { + msg.clear(); + msg.reserve(arr.length); + for (var obj : arr) { + proto.pack(msg.next(), obj); + } + } + + /** + * Pack a serialized protobuf double array message. + * + * @param msg protobuf double array message + * @param arr array of objects + */ + static void packArray(RepeatedDouble msg, double[] arr) { + msg.clear(); + msg.reserve(arr.length); + msg.addAll(arr); + } } diff --git a/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufSerializable.java b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufSerializable.java index ac75065659..b991e941ca 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufSerializable.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufSerializable.java @@ -10,6 +10,7 @@ import edu.wpi.first.util.WPISerializable; * Marker interface to indicate a class is serializable using Protobuf serialization. * *

While this cannot be enforced by the interface, any class implementing this interface should - * provide a public final static `proto` member variable. + * provide a public final static `proto` member variable, or a static final `getProto()` method if + * the class is generic. */ public interface ProtobufSerializable extends WPISerializable {} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/Struct.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/Struct.java index 05c7a4f654..763d61e829 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/struct/Struct.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/Struct.java @@ -4,6 +4,7 @@ package edu.wpi.first.util.struct; +import java.lang.reflect.Array; import java.nio.ByteBuffer; /** @@ -124,6 +125,73 @@ public interface Struct { throw new UnsupportedOperationException("object does not support unpackInto"); } + /** + * Deserializes an array from a raw struct serialized ByteBuffer starting at the current position. + * Will increment the ByteBuffer position by size * struct.size() bytes. Will not otherwise modify + * the ByteBuffer (e.g. byte order will not be changed). + * + * @param Object type + * @param bb ByteBuffer + * @param size Size of the array + * @param struct Struct implementation + * @return Deserialized array + */ + static T[] unpackArray(ByteBuffer bb, int size, Struct struct) { + @SuppressWarnings("unchecked") + T[] arr = (T[]) Array.newInstance(struct.getTypeClass(), size); + for (int i = 0; i < arr.length; i++) { + arr[i] = struct.unpack(bb); + } + return arr; + } + + /** + * Deserializes a double array from a raw struct serialized ByteBuffer starting at the current + * position. Will increment the ByteBuffer position by size * kSizeDouble bytes. Will not + * otherwise modify the ByteBuffer (e.g. byte order will not be changed). + * + * @param bb ByteBuffer + * @param size Size of the array + * @return Double array + */ + static double[] unpackDoubleArray(ByteBuffer bb, int size) { + double[] arr = new double[size]; + for (int i = 0; i < size; i++) { + arr[i] = bb.getDouble(); + } + return arr; + } + + /** + * Puts array contents to a ByteBuffer starting at the current position. Will increment the + * ByteBuffer position by size * struct.size() bytes. Will not otherwise modify the ByteBuffer + * (e.g. byte order will not be changed). + * + * @param Object type + * @param bb ByteBuffer + * @param arr Array to serialize + * @param struct Struct implementation + */ + static void packArray(ByteBuffer bb, T[] arr, Struct struct) { + for (T obj : arr) { + struct.pack(bb, obj); + } + } + + /** + * Puts array contents to a ByteBuffer starting at the current position. Will increment the + * ByteBuffer position by size * kSizeDouble bytes. Will not otherwise modify the ByteBuffer (e.g. + * byte order will not be changed). + * + * @param bb ByteBuffer + * @param arr Array to serialize + */ + static void packArray(ByteBuffer bb, double[] arr) { + for (double obj : arr) { + bb.putDouble(obj); + } + } + /** * Returns whether or not objects are immutable. Immutable objects must also be comparable using * the equals() method. Default implementation returns false. diff --git a/wpiutil/src/main/native/include/wpi/protobuf/Protobuf.h b/wpiutil/src/main/native/include/wpi/protobuf/Protobuf.h index 4eedcca297..fb8b7943c4 100644 --- a/wpiutil/src/main/native/include/wpi/protobuf/Protobuf.h +++ b/wpiutil/src/main/native/include/wpi/protobuf/Protobuf.h @@ -14,11 +14,16 @@ #include #include +#include "wpi/array.h" #include "wpi/function_ref.h" namespace google::protobuf { class Arena; class Message; +template +class RepeatedPtrField; +template +class RepeatedField; } // namespace google::protobuf namespace wpi { @@ -102,6 +107,50 @@ inline T UnpackProtobuf(const google::protobuf::Message& msg) { return Protobuf::Unpack(msg); } +/** + * Unpack a serialized protobuf array message. + * + * @tparam Proto element type of the protobuf array + * @tparam T object type + * @tparam N number of objects + * @param msg protobuf array message + * @return Deserialized array + */ +template Proto, + ProtobufSerializable T, size_t N> +wpi::array UnpackProtobufArray( + const google::protobuf::RepeatedPtrField& msg) { + if (N != std::dynamic_extent && msg.size() != N) { + // TODO + } + wpi::array arr(wpi::empty_array); + for (size_t i = 0; i < N; i++) { + arr[i] = wpi::UnpackProtobuf(msg.Get(i)); + } + return arr; +} + +/** + * Unpack a serialized protobuf array message. + * + * @tparam T element type of the protobuf array + * @tparam N number of objects + * @param msg protobuf array message + * @return Deserialized array + */ +template +wpi::array UnpackProtobufArray( + const google::protobuf::RepeatedField& msg) { + if (N != std::dynamic_extent && msg.size() != N) { + // TODO + } + wpi::array arr(wpi::empty_array); + for (size_t i = 0; i < N; i++) { + arr[i] = msg.Get(i); + } + return arr; +} + /** * Pack a serialized protobuf message. * @@ -113,6 +162,42 @@ inline void PackProtobuf(google::protobuf::Message* msg, const T& value) { Protobuf>::Pack(msg, value); } +/** + * Pack a serialized protobuf array message. + * + * @tparam Proto element type of the protobuf array + * @tparam T object type + * @tparam N number of objects + * @param msg protobuf message (mutable, output) + * @param arr array of objects + */ +template Proto, + ProtobufSerializable T, size_t N> +void PackProtobufArray(google::protobuf::RepeatedPtrField* msg, + const wpi::array& arr) { + msg->Clear(); + msg->Reserve(N); + for (const auto& obj : arr) { + PackProtobuf(msg->Add(), obj); + } +} + +/** + * Pack a serialized protobuf array message. + * + * @tparam T object type + * @tparam N number of objects + * @param msg protobuf message (mutable, output) + * @param arr array of objects + */ +template +void PackProtobufArray(google::protobuf::RepeatedField* msg, + const wpi::array& arr) { + msg->Clear(); + msg->Reserve(N); + msg->Add(arr.begin(), arr.end()); +} + /** * Unpack a serialized struct into an existing object, overwriting its contents. * diff --git a/wpiutil/src/main/native/include/wpi/struct/Struct.h b/wpiutil/src/main/native/include/wpi/struct/Struct.h index 02f28f10ef..0de507e743 100644 --- a/wpiutil/src/main/native/include/wpi/struct/Struct.h +++ b/wpiutil/src/main/native/include/wpi/struct/Struct.h @@ -18,6 +18,7 @@ #include #include "wpi/Endian.h" +#include "wpi/array.h" #include "wpi/bit.h" #include "wpi/ct_string.h" #include "wpi/function_ref.h" @@ -156,6 +157,37 @@ inline T UnpackStruct(std::span data, const I&... info) { return S::Unpack(data.subspan(Offset), info...); } +/** + * Unpack a serialized struct array starting at a given offset within the data. + * This is primarily useful in unpack implementations to unpack nested struct + * arrays. + * + * @tparam T object type + * @tparam Offset starting offset + * @tparam N number of objects + * @param data raw struct data + * @return Desrialized array + */ +template +inline wpi::array UnpackStructArray(std::span data) { + if (is_constexpr([] { Struct>::GetSize(); })) { + constexpr auto StructSize = Struct>::GetSize(); + wpi::array arr(wpi::empty_array); + [&](std::index_sequence) { + ((arr[Is] = UnpackStruct(data)), ...); + }(std::make_index_sequence{}); + return arr; + } else { + auto size = Struct>::GetSize(); + wpi::array arr(wpi::empty_array); + for (size_t i = 0; i < N; i++) { + arr[i] = UnpackStruct(data); + data = data.subspan(size); + } + return arr; + } +} + /** * Pack a serialized struct. * @@ -188,6 +220,33 @@ inline void PackStruct(std::span data, T&& value, const I&... info) { S::Pack(data.subspan(Offset), std::forward(value), info...); } +/** + * Pack a serialized struct array starting at a given offset within the data. + * This is primarily useful in pack implementations to pack nested struct + * arrays. + * + * @tparam Offset starting offset + * @tparam N number of objects + * @param data struct storage (mutable, output) + * @param arr array of object + */ +template +inline void PackStructArray(std::span data, + const wpi::array& arr) { + if (is_constexpr([] { Struct>::GetSize(); })) { + constexpr auto StructSize = Struct>::GetSize(); + [&](std::index_sequence) { + (PackStruct(data, arr[Is]), ...); + }(std::make_index_sequence{}); + } else { + auto size = Struct>::GetSize(); + for (auto&& val : arr) { + PackStruct(data, val); + data = data.subspan(size); + } + } +} + /** * Unpack a serialized struct into an existing object, overwriting its contents. *