Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Joseph Eng
2024-07-29 07:55:44 -07:00
committed by GitHub
parent 3e1e3fb4ca
commit 073192d513
112 changed files with 3989 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
docs/build.gradle
View File

@@ -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)

4
wpimath/CMakeLists.txt
View File

@@ -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:$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/exports.def>
)
endif()
wpilib_target_warnings(wpimath)
target_link_libraries(wpimath wpiutil)

135
wpimath/src/generated/main/java/edu/wpi/first/math/proto/Controller.java generated
View File

@@ -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;
/**
* <code>optional double dt = 4;</code>
*/
private double dt;
private ProtobufSimpleMotorFeedforward() {
}
@@ -1697,6 +1703,43 @@ public final class Controller {
return this;
}
/**
* <code>optional double dt = 4;</code>
* @return whether the dt field is set
*/
public boolean hasDt() {
return (bitField0_ & 0x00000008) != 0;
}
/**
* <code>optional double dt = 4;</code>
* @return this
*/
public ProtobufSimpleMotorFeedforward clearDt() {
bitField0_ &= ~0x00000008;
dt = 0D;
return this;
}
/**
* <code>optional double dt = 4;</code>
* @return the dt
*/
public double getDt() {
return dt;
}
/**
* <code>optional double dt = 4;</code>
* @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");
}
}

37
wpimath/src/main/java/edu/wpi/first/math/Matrix.java
View File

@@ -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 <R> The number of rows in this matrix.
* @param <C> The number of columns in this matrix.
*/
public class Matrix<R extends Num, C extends Num> {
public class Matrix<R extends Num, C extends Num>
implements ProtobufSerializable, StructSerializable {
/** Storage for underlying EJML matrix. */
protected final SimpleMatrix m_storage;
@@ -738,4 +745,32 @@ public class Matrix<R extends Num, C extends Num> {
public int hashCode() {
return Objects.hash(m_storage);
}
/**
* Creates an implementation of the {@link Protobuf} interface for matrices.
*
* @param <R> The number of rows of the matrices this serializer processes.
* @param <C> 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 <R extends Num, C extends Num> MatrixProto<R, C> getProto(
Nat<R> rows, Nat<C> cols) {
return new MatrixProto<>(rows, cols);
}
/**
* Creates an implementation of the {@link Struct} interfaces for matrices.
*
* @param <R> The number of rows of the matrices this serializer processes.
* @param <C> 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 <R extends Num, C extends Num> MatrixStruct<R, C> getStruct(
Nat<R> rows, Nat<C> cols) {
return new MatrixStruct<>(rows, cols);
}
}

31
wpimath/src/main/java/edu/wpi/first/math/Vector.java
View File

@@ -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 <R> The number of rows in this matrix.
*/
public class Vector<R extends Num> extends Matrix<R, N1> {
public class Vector<R extends Num> extends Matrix<R, N1>
implements ProtobufSerializable, StructSerializable {
/**
* Constructs an empty zero vector of the given dimensions.
*
@@ -151,4 +158,26 @@ public class Vector<R extends Num> extends Matrix<R, N1> {
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 <R> 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 <R extends Num> VectorProto<R> getProto(Nat<R> rows) {
return new VectorProto<>(rows);
}
/**
* Creates an implementation of the {@link Struct} interface for vectors.
*
* @param <R> 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 <R extends Num> VectorStruct<R> getStruct(Nat<R> rows) {
return new VectorStruct<>(rows);
}
}

35
wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java
View File

@@ -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<N2, N2, N2> 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();
}

12
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -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();
}

42
wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProto.java Normal file
View File

@@ -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<DifferentialDriveFeedforward, ProtobufDifferentialDriveFeedforward> {
@Override
public Class<DifferentialDriveFeedforward> 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);
}
}

38
wpimath/src/main/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProto.java Normal file
View File

@@ -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<SimpleMotorFeedforward, ProtobufSimpleMotorFeedforward> {
@Override
public Class<SimpleMotorFeedforward> 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());
}
}

49
wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStruct.java Normal file
View File

@@ -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<DifferentialDriveFeedforward> {
@Override
public Class<DifferentialDriveFeedforward> 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);
}
}

48
wpimath/src/main/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStruct.java Normal file
View File

@@ -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<SimpleMotorFeedforward> {
@Override
public Class<SimpleMotorFeedforward> 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());
}
}

14
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
View File

@@ -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();
}

33
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -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<SwerveModuleState[], SwerveModulePosition[]> {
implements Kinematics<SwerveModuleState[], SwerveModulePosition[]>,
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);
}
}

42
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProto.java Normal file
View File

@@ -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<MecanumDriveMotorVoltages, ProtobufMecanumDriveMotorVoltages> {
@Override
public Class<MecanumDriveMotorVoltages> 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);
}
}

39
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProto.java Normal file
View File

@@ -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<SwerveDriveKinematics, ProtobufSwerveDriveKinematics> {
@Override
public Class<SwerveDriveKinematics> 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);
}
}

48
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStruct.java Normal file
View File

@@ -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<MecanumDriveMotorVoltages> {
@Override
public Class<MecanumDriveMotorVoltages> 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);
}
}

58
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStruct.java Normal file
View File

@@ -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<SwerveDriveKinematics> {
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<SwerveDriveKinematics> 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);
}
}

73
wpimath/src/main/java/edu/wpi/first/math/proto/MatrixProto.java Normal file
View File

@@ -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<R extends Num, C extends Num>
implements Protobuf<Matrix<R, C>, ProtobufMatrix> {
private final Nat<R> m_rows;
private final Nat<C> 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<R> rows, Nat<C> cols) {
m_rows = rows;
m_cols = cols;
}
@Override
public Class<Matrix<R, C>> getTypeClass() {
@SuppressWarnings("unchecked")
var clazz = (Class<Matrix<R, C>>) (Class<?>) Matrix.class;
return clazz;
}
@Override
public Descriptor getDescriptor() {
return ProtobufMatrix.getDescriptor();
}
@Override
public ProtobufMatrix createMessage() {
return ProtobufMatrix.newInstance();
}
@Override
public Matrix<R, C> 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<R, C> value) {
msg.setNumRows(value.getNumRows());
msg.setNumCols(value.getNumCols());
Protobuf.packArray(msg.getMutableData(), value.getData());
}
}

64
wpimath/src/main/java/edu/wpi/first/math/proto/VectorProto.java Normal file
View File

@@ -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<R extends Num> implements Protobuf<Vector<R>, ProtobufVector> {
private final Nat<R> m_rows;
/**
* Constructs the {@link Protobuf} implementation.
*
* @param rows The number of rows of the vectors this serializer processes.
*/
public VectorProto(Nat<R> rows) {
m_rows = rows;
}
@Override
public Class<Vector<R>> getTypeClass() {
@SuppressWarnings("unchecked")
var clazz = (Class<Vector<R>>) (Class<?>) Vector.class;
return clazz;
}
@Override
public Descriptor getDescriptor() {
return ProtobufVector.getDescriptor();
}
@Override
public ProtobufVector createMessage() {
return ProtobufVector.newInstance();
}
@Override
public Vector<R> 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<R>(storage);
}
@Override
public void pack(ProtobufVector msg, Vector<R> value) {
Protobuf.packArray(msg.getMutableRows(), value.getData());
}
}

29
wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
View File

@@ -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();
}

30
wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
View File

@@ -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();
}

45
wpimath/src/main/java/edu/wpi/first/math/spline/proto/CubicHermiteSplineProto.java Normal file
View File

@@ -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<CubicHermiteSpline, ProtobufCubicHermiteSpline> {
@Override
public Class<CubicHermiteSpline> 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);
}
}

45
wpimath/src/main/java/edu/wpi/first/math/spline/proto/QuinticHermiteSplineProto.java Normal file
View File

@@ -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<QuinticHermiteSpline, ProtobufQuinticHermiteSpline> {
@Override
public Class<QuinticHermiteSpline> 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);
}
}

48
wpimath/src/main/java/edu/wpi/first/math/spline/struct/CubicHermiteSplineStruct.java Normal file
View File

@@ -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<CubicHermiteSpline> {
@Override
public Class<CubicHermiteSpline> 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);
}
}

48
wpimath/src/main/java/edu/wpi/first/math/spline/struct/QuinticHermiteSplineStruct.java Normal file
View File

@@ -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<QuinticHermiteSpline> {
@Override
public Class<QuinticHermiteSpline> 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);
}
}

61
wpimath/src/main/java/edu/wpi/first/math/struct/MatrixStruct.java Normal file
View File

@@ -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<R extends Num, C extends Num> implements Struct<Matrix<R, C>> {
private final Nat<R> m_rows;
private final Nat<C> 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<R> rows, Nat<C> cols) {
m_rows = rows;
m_cols = cols;
}
@Override
public Class<Matrix<R, C>> getTypeClass() {
@SuppressWarnings("unchecked")
var clazz = (Class<Matrix<R, C>>) (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<R, C> 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<R, C> value) {
Struct.packArray(bb, value.getData());
}
}

58
wpimath/src/main/java/edu/wpi/first/math/struct/VectorStruct.java Normal file
View File

@@ -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<R extends Num> implements Struct<Vector<R>> {
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<R> rows) {
m_rows = rows.getNum();
}
@Override
public Class<Vector<R>> getTypeClass() {
@SuppressWarnings("unchecked")
var clazz = (Class<Vector<R>>) (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<R> unpack(ByteBuffer bb) {
var storage = new SimpleMatrix(Struct.unpackDoubleArray(bb, m_rows));
return new Vector<R>(storage);
}
@Override
public void pack(ByteBuffer bb, Vector<R> value) {
Struct.packArray(bb, value.getData());
}
}

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

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

99
wpimath/src/main/java/edu/wpi/first/math/system/proto/LinearSystemProto.java Normal file
View File

@@ -0,0 +1,99 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.system.proto;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.proto.System.ProtobufLinearSystem;
import edu.wpi.first.math.proto.Wpimath.ProtobufMatrix;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public final class LinearSystemProto<States extends Num, Inputs extends Num, Outputs extends Num>
implements Protobuf<LinearSystem<States, Inputs, Outputs>, ProtobufLinearSystem> {
private final Nat<States> m_states;
private final Nat<Inputs> m_inputs;
private final Nat<Outputs> m_outputs;
private final Protobuf<Matrix<States, States>, ProtobufMatrix> m_AProto;
private final Protobuf<Matrix<States, Inputs>, ProtobufMatrix> m_BProto;
private final Protobuf<Matrix<Outputs, States>, ProtobufMatrix> m_CProto;
private final Protobuf<Matrix<Outputs, Inputs>, ProtobufMatrix> m_DProto;
/**
* Constructs the {@link Protobuf} implementation.
*
* @param states The number of states of the linear systems this serializer processes.
* @param inputs The number of inputs of the linear systems this serializer processes.
* @param outputs The number of outputs of the linear systems this serializer processes.
*/
public LinearSystemProto(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
m_states = states;
m_inputs = inputs;
m_outputs = outputs;
m_AProto = Matrix.getProto(states, states);
m_BProto = Matrix.getProto(states, inputs);
m_CProto = Matrix.getProto(outputs, states);
m_DProto = Matrix.getProto(outputs, inputs);
}
@Override
public Class<LinearSystem<States, Inputs, Outputs>> getTypeClass() {
@SuppressWarnings("unchecked")
var clazz = (Class<LinearSystem<States, Inputs, Outputs>>) (Class<?>) LinearSystem.class;
return clazz;
}
@Override
public Descriptor getDescriptor() {
return ProtobufLinearSystem.getDescriptor();
}
@Override
public ProtobufLinearSystem createMessage() {
return ProtobufLinearSystem.newInstance();
}
@Override
public LinearSystem<States, Inputs, Outputs> unpack(ProtobufLinearSystem msg) {
if (msg.getNumStates() != m_states.getNum()
|| msg.getNumInputs() != m_inputs.getNum()
|| msg.getNumOutputs() != m_outputs.getNum()) {
throw new IllegalArgumentException(
"Tried to unpack msg "
+ msg
+ " with "
+ msg.getNumStates()
+ " states and "
+ msg.getNumInputs()
+ " inputs and "
+ msg.getNumOutputs()
+ " outputs into LinearSystem with "
+ m_states.getNum()
+ " states "
+ m_inputs.getNum()
+ " inputs "
+ m_outputs.getNum()
+ " outputs");
}
return new LinearSystem<>(
m_AProto.unpack(msg.getA()),
m_BProto.unpack(msg.getB()),
m_CProto.unpack(msg.getC()),
m_DProto.unpack(msg.getD()));
}
@Override
public void pack(ProtobufLinearSystem msg, LinearSystem<States, Inputs, Outputs> value) {
msg.setNumStates(m_states.getNum())
.setNumInputs(m_inputs.getNum())
.setNumOutputs(m_outputs.getNum());
m_AProto.pack(msg.getMutableA(), value.getA());
m_BProto.pack(msg.getMutableB(), value.getB());
m_CProto.pack(msg.getMutableC(), value.getC());
m_DProto.pack(msg.getMutableD(), value.getD());
}
}

89
wpimath/src/main/java/edu/wpi/first/math/system/struct/LinearSystemStruct.java Normal file
View File

@@ -0,0 +1,89 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.system.struct;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.struct.MatrixStruct;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.util.struct.Struct;
import java.nio.ByteBuffer;
public final class LinearSystemStruct<States extends Num, Inputs extends Num, Outputs extends Num>
implements Struct<LinearSystem<States, Inputs, Outputs>> {
private final int m_states;
private final int m_inputs;
private final int m_outputs;
private final MatrixStruct<States, States> m_AStruct;
private final MatrixStruct<States, Inputs> m_BStruct;
private final MatrixStruct<Outputs, States> m_CStruct;
private final MatrixStruct<Outputs, Inputs> m_DStruct;
/**
* Constructs the {@link Struct} implementation.
*
* @param states The number of states of the linear systems this serializer processes.
* @param inputs The number of inputs of the linear systems this serializer processes.
* @param outputs The number of outputs of the linear systems this serializer processes.
*/
public LinearSystemStruct(Nat<States> states, Nat<Inputs> inputs, Nat<Outputs> outputs) {
m_states = states.getNum();
m_inputs = inputs.getNum();
m_outputs = outputs.getNum();
m_AStruct = Matrix.getStruct(states, states);
m_BStruct = Matrix.getStruct(states, inputs);
m_CStruct = Matrix.getStruct(outputs, states);
m_DStruct = Matrix.getStruct(outputs, inputs);
}
@Override
public Class<LinearSystem<States, Inputs, Outputs>> getTypeClass() {
@SuppressWarnings("unchecked")
var clazz = (Class<LinearSystem<States, Inputs, Outputs>>) (Class<?>) LinearSystem.class;
return clazz;
}
@Override
public String getTypeName() {
return "LinearSystem__" + m_states + "_" + m_inputs + "_" + m_outputs;
}
@Override
public int getSize() {
return m_AStruct.getSize() + m_BStruct.getSize() + m_CStruct.getSize() + m_DStruct.getSize();
}
@Override
public String getSchema() {
return m_AStruct.getTypeName()
+ " a;"
+ m_BStruct.getTypeName()
+ " b;"
+ m_CStruct.getTypeName()
+ " c;"
+ m_DStruct.getTypeName()
+ " d";
}
@Override
public Struct<?>[] getNested() {
return new Struct<?>[] {m_AStruct, m_BStruct, m_CStruct, m_DStruct};
}
@Override
public LinearSystem<States, Inputs, Outputs> unpack(ByteBuffer bb) {
return new LinearSystem<>(
m_AStruct.unpack(bb), m_BStruct.unpack(bb), m_CStruct.unpack(bb), m_DStruct.unpack(bb));
}
@Override
public void pack(ByteBuffer bb, LinearSystem<States, Inputs, Outputs> value) {
m_AStruct.pack(bb, value.getA());
m_BStruct.pack(bb, value.getB());
m_CStruct.pack(bb, value.getC());
m_DStruct.pack(bb, value.getD());
}
}

14
wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
View File

@@ -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,

36
wpimath/src/main/native/cpp/controller/proto/DifferentialDriveFeedforwardProto.cpp Normal file
View File

@@ -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 <wpi/ProtoHelper.h>
#include "controller.pb.h"
google::protobuf::Message* wpi::Protobuf<
frc::DifferentialDriveFeedforward>::New(google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufDifferentialDriveFeedforward>(
arena);
}
frc::DifferentialDriveFeedforward
wpi::Protobuf<frc::DifferentialDriveFeedforward>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufDifferentialDriveFeedforward*>(
&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<frc::DifferentialDriveFeedforward>::Pack(
google::protobuf::Message* msg,
const frc::DifferentialDriveFeedforward& value) {
auto m = static_cast<wpi::proto::ProtobufDifferentialDriveFeedforward*>(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());
}

29
wpimath/src/main/native/cpp/controller/struct/DifferentialDriveFeedforwardStruct.cpp Normal file
View File

@@ -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<const uint8_t> data) {
return {
decltype(1_V / 1_mps){wpi::UnpackStruct<double, kKvLinearOff>(data)},
decltype(1_V / 1_mps_sq){wpi::UnpackStruct<double, kKaLinearOff>(data)},
decltype(1_V / 1_mps){wpi::UnpackStruct<double, kKvAngularOff>(data)},
decltype(1_V / 1_mps_sq){wpi::UnpackStruct<double, kKaAngularOff>(data)}};
}
void wpi::Struct<frc::DifferentialDriveFeedforward>::Pack(
std::span<uint8_t> data, const frc::DifferentialDriveFeedforward& value) {
wpi::PackStruct<kKvLinearOff>(data, value.m_kVLinear.value());
wpi::PackStruct<kKaLinearOff>(data, value.m_kALinear.value());
wpi::PackStruct<kKvAngularOff>(data, value.m_kVAngular.value());
wpi::PackStruct<kKaAngularOff>(data, value.m_kAAngular.value());
}

35
wpimath/src/main/native/cpp/spline/proto/CubicHermiteSplineProto.cpp Normal file
View File

@@ -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 <wpi/ProtoHelper.h>
#include "spline.pb.h"
google::protobuf::Message* wpi::Protobuf<frc::CubicHermiteSpline>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufCubicHermiteSpline>(arena);
}
frc::CubicHermiteSpline wpi::Protobuf<frc::CubicHermiteSpline>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufCubicHermiteSpline*>(&msg);
return frc::CubicHermiteSpline{
wpi::UnpackProtobufArray<double, 2>(m->x_initial()),
wpi::UnpackProtobufArray<double, 2>(m->x_final()),
wpi::UnpackProtobufArray<double, 2>(m->y_initial()),
wpi::UnpackProtobufArray<double, 2>(m->y_final())};
}
void wpi::Protobuf<frc::CubicHermiteSpline>::Pack(
google::protobuf::Message* msg, const frc::CubicHermiteSpline& value) {
auto m = static_cast<wpi::proto::ProtobufCubicHermiteSpline*>(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);
}

35
wpimath/src/main/native/cpp/spline/proto/QuinticHermiteSplineProto.cpp Normal file
View File

@@ -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 <wpi/ProtoHelper.h>
#include "spline.pb.h"
google::protobuf::Message* wpi::Protobuf<frc::QuinticHermiteSpline>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufQuinticHermiteSpline>(arena);
}
frc::QuinticHermiteSpline wpi::Protobuf<frc::QuinticHermiteSpline>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufQuinticHermiteSpline*>(&msg);
return frc::QuinticHermiteSpline{
wpi::UnpackProtobufArray<double, 3>(m->x_initial()),
wpi::UnpackProtobufArray<double, 3>(m->x_final()),
wpi::UnpackProtobufArray<double, 3>(m->y_initial()),
wpi::UnpackProtobufArray<double, 3>(m->y_final())};
}
void wpi::Protobuf<frc::QuinticHermiteSpline>::Pack(
google::protobuf::Message* msg, const frc::QuinticHermiteSpline& value) {
auto m = static_cast<wpi::proto::ProtobufQuinticHermiteSpline*>(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);
}

31
wpimath/src/main/native/cpp/spline/struct/CubicHermiteSplineStruct.cpp Normal file
View File

@@ -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<frc::CubicHermiteSpline>::Unpack(
std::span<const uint8_t> data) {
return frc::CubicHermiteSpline{
wpi::UnpackStructArray<double, kXInitialOff, 2>(data),
wpi::UnpackStructArray<double, kXFinalOff, 2>(data),
wpi::UnpackStructArray<double, kYInitialOff, 2>(data),
wpi::UnpackStructArray<double, kYFinalOff, 2>(data)};
}
void wpi::Struct<frc::CubicHermiteSpline>::Pack(
std::span<uint8_t> data, const frc::CubicHermiteSpline& value) {
wpi::PackStructArray<kXInitialOff, 2>(data,
value.GetInitialControlVector().x);
wpi::PackStructArray<kXFinalOff, 2>(data, value.GetFinalControlVector().x);
wpi::PackStructArray<kYInitialOff, 2>(data,
value.GetInitialControlVector().y);
wpi::PackStructArray<kYFinalOff, 2>(data, value.GetFinalControlVector().y);
}

31
wpimath/src/main/native/cpp/spline/struct/QuinticHermiteSplineStruct.cpp Normal file
View File

@@ -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<frc::QuinticHermiteSpline>::Unpack(
std::span<const uint8_t> data) {
return frc::QuinticHermiteSpline{
wpi::UnpackStructArray<double, kXInitialOff, 3>(data),
wpi::UnpackStructArray<double, kXFinalOff, 3>(data),
wpi::UnpackStructArray<double, kYInitialOff, 3>(data),
wpi::UnpackStructArray<double, kYFinalOff, 3>(data)};
}
void wpi::Struct<frc::QuinticHermiteSpline>::Pack(
std::span<uint8_t> data, const frc::QuinticHermiteSpline& value) {
wpi::PackStructArray<kXInitialOff, 3>(data,
value.GetInitialControlVector().x);
wpi::PackStructArray<kXFinalOff, 3>(data, value.GetFinalControlVector().x);
wpi::PackStructArray<kYInitialOff, 3>(data,
value.GetInitialControlVector().y);
wpi::PackStructArray<kYFinalOff, 3>(data, value.GetFinalControlVector().y);
}

9
wpimath/src/main/native/exports.def Normal file
View File

@@ -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

8
wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h
View File

@@ -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"

19
wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveFeedforwardProto.h Normal file
View File

@@ -0,0 +1,19 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/controller/DifferentialDriveFeedforward.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveFeedforward> {
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);
};

24
wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h Normal file
View File

@@ -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 <wpi/protobuf/Protobuf.h>
#include "frc/controller/SimpleMotorFeedforward.h"
#include "units/length.h"
// Everything is converted into units for
// frc::SimpleMotorFeedforward<units::meters>
template <class Distance>
struct wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static frc::SimpleMotorFeedforward<Distance> Unpack(
const google::protobuf::Message& msg);
static void Pack(google::protobuf::Message* msg,
const frc::SimpleMotorFeedforward<Distance>& value);
};
#include "frc/controller/proto/SimpleMotorFeedforwardProto.inc"

45
wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.inc Normal file
View File

@@ -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 <wpi/ProtoHelper.h>
#include "controller.pb.h"
#include "frc/controller/proto/SimpleMotorFeedforwardProto.h"
template <class Distance>
google::protobuf::Message*
wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufSimpleMotorFeedforward>(arena);
}
template <class Distance>
frc::SimpleMotorFeedforward<Distance>
wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufSimpleMotorFeedforward*>(&msg);
return {units::volt_t{m->ks()},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
m->kv()},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
m->ka()},
units::second_t{m->dt()}};
}
template <class Distance>
void wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::Pack(
google::protobuf::Message* msg,
const frc::SimpleMotorFeedforward<Distance>& value) {
auto m = static_cast<wpi::proto::ProtobufSimpleMotorFeedforward*>(msg);
m->set_ks(value.GetKs().value());
m->set_kv(units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
value.GetKv()}
.value());
m->set_ka(units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
value.GetKa()}
.value());
m->set_dt(units::second_t{value.GetDt()}.value());
}

29
wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveFeedforwardStruct.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/controller/DifferentialDriveFeedforward.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveFeedforward> {
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<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::DifferentialDriveFeedforward& value);
};
static_assert(wpi::StructSerializable<frc::DifferentialDriveFeedforward>);

36
wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h Normal file
View File

@@ -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 <wpi/struct/Struct.h>
#include "frc/controller/SimpleMotorFeedforward.h"
#include "units/length.h"
// Everything is converted into units for
// frc::SimpleMotorFeedforward<units::meters>
template <class Distance>
struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
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<Distance> Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::SimpleMotorFeedforward<Distance>& value);
};
static_assert(
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::meters>>);
static_assert(
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::feet>>);
#include "frc/controller/struct/SimpleMotorFeedforwardStruct.inc"

45
wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.inc Normal file
View File

@@ -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 <class Distance>
frc::SimpleMotorFeedforward<Distance>
wpi::Struct<frc::SimpleMotorFeedforward<Distance>>::Unpack(
std::span<const uint8_t> 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<double, kKsOff>(data)},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
wpi::UnpackStruct<double, kKvOff>(data)},
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
wpi::UnpackStruct<double, kKaOff>(data)},
units::second_t{wpi::UnpackStruct<double, kDtOff>(data)}};
}
template <class Distance>
void wpi::Struct<frc::SimpleMotorFeedforward<Distance>>::Pack(
std::span<uint8_t> data,
const frc::SimpleMotorFeedforward<Distance>& 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<kKsOff>(data, value.GetKs().value());
wpi::PackStruct<kKvOff>(
data,
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
value.GetKv()}
.value());
wpi::PackStruct<kKaOff>(
data,
units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
value.GetKa()}
.value());
wpi::PackStruct<kDtOff>(data, units::second_t{value.GetDt()}.value());
}

6
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -299,10 +299,14 @@ class SwerveDriveKinematics
return {result};
}
const wpi::array<Translation2d, NumModules> GetModules() const {
return m_modules;
}
private:
wpi::array<Translation2d, NumModules> m_modules;
mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
wpi::array<Translation2d, NumModules> m_modules;
mutable wpi::array<Rotation2d, NumModules> m_moduleHeadings;
mutable Translation2d m_previousCoR;

20
wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.h Normal file
View File

@@ -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 <wpi/protobuf/Protobuf.h>
#include "frc/kinematics/SwerveDriveKinematics.h"
template <size_t NumModules>
struct wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static frc::SwerveDriveKinematics<NumModules> Unpack(
const google::protobuf::Message& msg);
static void Pack(google::protobuf::Message* msg,
const frc::SwerveDriveKinematics<NumModules>& value);
};
#include "frc/kinematics/proto/SwerveDriveKinematicsProto.inc"

44
wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.inc Normal file
View File

@@ -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 <stdexcept>
#include <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include "frc/kinematics/proto/SwerveDriveKinematicsProto.h"
#include "kinematics.pb.h"
template <size_t NumModules>
google::protobuf::Message*
wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufSwerveDriveKinematics>(arena);
}
template <size_t NumModules>
frc::SwerveDriveKinematics<NumModules>
wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufSwerveDriveKinematics*>(&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<NumModules>{
wpi::UnpackProtobufArray<wpi::proto::ProtobufTranslation2d,
frc::Translation2d, NumModules>(m->modules())};
}
template <size_t NumModules>
void wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::Pack(
google::protobuf::Message* msg,
const frc::SwerveDriveKinematics<NumModules>& value) {
auto m = static_cast<wpi::proto::ProtobufSwerveDriveKinematics*>(msg);
wpi::PackProtobufArray(m->mutable_modules(), value.GetModules());
}

41
wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.h Normal file
View File

@@ -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 <fmt/format.h>
#include <wpi/ct_string.h>
#include <wpi/struct/Struct.h>
#include "frc/kinematics/SwerveDriveKinematics.h"
template <size_t NumModules>
struct wpi::Struct<frc::SwerveDriveKinematics<NumModules>> {
static constexpr ct_string kTypeName = wpi::Concat(
"SwerveDriveKinematics__"_ct_string, wpi::NumToCtString<NumModules>());
static constexpr std::string_view GetTypeName() { return kTypeName; }
static constexpr size_t GetSize() {
return NumModules * wpi::Struct<frc::Translation2d>::GetSize();
}
static constexpr ct_string kSchema =
wpi::Concat("Translation2d modules["_ct_string,
wpi::NumToCtString<NumModules>(), "]"_ct_string);
static constexpr std::string_view GetSchema() { return kSchema; }
static frc::SwerveDriveKinematics<NumModules> Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::SwerveDriveKinematics<NumModules>& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
}
};
static_assert(wpi::StructSerializable<frc::SwerveDriveKinematics<4>>);
static_assert(wpi::HasNestedStruct<frc::SwerveDriveKinematics<4>>);
static_assert(wpi::StructSerializable<frc::SwerveDriveKinematics<3>>);
static_assert(wpi::HasNestedStruct<frc::SwerveDriveKinematics<3>>);
#include "frc/kinematics/struct/SwerveDriveKinematicsStruct.inc"

25
wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.inc Normal file
View File

@@ -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 <size_t NumModules>
frc::SwerveDriveKinematics<NumModules>
wpi::Struct<frc::SwerveDriveKinematics<NumModules>>::Unpack(
std::span<const uint8_t> data) {
constexpr size_t kModulesOff = 0;
return frc::SwerveDriveKinematics<NumModules>{
wpi::UnpackStructArray<frc::Translation2d, kModulesOff, NumModules>(
data)};
}
template <size_t NumModules>
void wpi::Struct<frc::SwerveDriveKinematics<NumModules>>::Pack(
std::span<uint8_t> data,
const frc::SwerveDriveKinematics<NumModules>& value) {
constexpr size_t kModulesOff = 0;
wpi::PackStructArray<kModulesOff, NumModules>(data, value.GetModules());
}

22
wpimath/src/main/native/include/frc/proto/MatrixProto.h Normal file
View File

@@ -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 <wpi/protobuf/Protobuf.h>
#include "frc/EigenCore.h"
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
struct wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> Unpack(
const google::protobuf::Message& msg);
static void Pack(
google::protobuf::Message* msg,
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value);
};
#include "frc/proto/MatrixProto.inc"

60
wpimath/src/main/native/include/frc/proto/MatrixProto.inc Normal file
View File

@@ -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 <stdexcept>
#include <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include "frc/proto/MatrixProto.h"
#include "wpimath.pb.h"
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
google::protobuf::Message*
wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufMatrix>(arena);
}
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>
wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufMatrix*>(&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<Rows, Cols, Options, MaxRows, MaxCols> mat;
for (int i = 0; i < Rows * Cols; i++) {
mat(i) = m->data(i);
}
return mat;
}
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
void wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Pack(
google::protobuf::Message* msg,
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
auto m = static_cast<wpi::proto::ProtobufMatrix*>(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));
}
}

21
wpimath/src/main/native/include/frc/proto/VectorProto.h Normal file
View File

@@ -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 <wpi/protobuf/Protobuf.h>
#include "frc/EigenCore.h"
template <int Size, int Options, int MaxRows, int MaxCols>
struct wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> Unpack(
const google::protobuf::Message& msg);
static void Pack(
google::protobuf::Message* msg,
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value);
};
#include "frc/proto/VectorProto.inc"

49
wpimath/src/main/native/include/frc/proto/VectorProto.inc Normal file
View File

@@ -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 <stdexcept>
#include <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include "frc/proto/VectorProto.h"
#include "wpimath.pb.h"
template <int Size, int Options, int MaxRows, int MaxCols>
google::protobuf::Message*
wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufVector>(arena);
}
template <int Size, int Options, int MaxRows, int MaxCols>
frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>
wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufVector*>(&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<Size, 1, Options, MaxRows, MaxCols> vec;
for (int i = 0; i < Size; i++) {
vec(i) = m->rows(i);
}
return vec;
}
template <int Size, int Options, int MaxRows, int MaxCols>
void wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Pack(
google::protobuf::Message* msg,
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
auto m = static_cast<wpi::proto::ProtobufVector*>(msg);
m->clear_rows();
for (int i = 0; i < Size; i++) {
m->add_rows(value(i));
}
}

3
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -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"

3
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -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"

18
wpimath/src/main/native/include/frc/spline/proto/CubicHermiteSplineProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/spline/CubicHermiteSpline.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::CubicHermiteSpline> {
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);
};

18
wpimath/src/main/native/include/frc/spline/proto/QuinticHermiteSplineProto.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#include <wpi/protobuf/Protobuf.h>
#include "frc/spline/QuinticHermiteSpline.h"
template <>
struct WPILIB_DLLEXPORT wpi::Protobuf<frc::QuinticHermiteSpline> {
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);
};

28
wpimath/src/main/native/include/frc/spline/struct/CubicHermiteSplineStruct.h Normal file
View File

@@ -0,0 +1,28 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/spline/CubicHermiteSpline.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::CubicHermiteSpline> {
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<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::CubicHermiteSpline& value);
};
static_assert(wpi::StructSerializable<frc::CubicHermiteSpline>);

28
wpimath/src/main/native/include/frc/spline/struct/QuinticHermiteSplineStruct.h Normal file
View File

@@ -0,0 +1,28 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/struct/Struct.h>
#include "frc/spline/QuinticHermiteSpline.h"
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::QuinticHermiteSpline> {
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<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::QuinticHermiteSpline& value);
};
static_assert(wpi::StructSerializable<frc::QuinticHermiteSpline>);

36
wpimath/src/main/native/include/frc/struct/MatrixStruct.h Normal file
View File

@@ -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 <fmt/format.h>
#include <wpi/ct_string.h>
#include <wpi/struct/Struct.h>
#include "frc/EigenCore.h"
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
struct wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
static constexpr ct_string kTypeName =
wpi::Concat("Matrix__"_ct_string, wpi::NumToCtString<Rows>(),
"_"_ct_string, wpi::NumToCtString<Cols>());
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<Rows * Cols>(),
"]"_ct_string);
static constexpr std::string_view GetSchema() { return kSchema; }
static frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> Unpack(
std::span<const uint8_t> data);
static void Pack(
std::span<uint8_t> data,
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value);
};
static_assert(wpi::StructSerializable<frc::Matrixd<1, 2>>);
static_assert(wpi::StructSerializable<frc::Matrixd<3, 3>>);
#include "frc/struct/MatrixStruct.inc"

35
wpimath/src/main/native/include/frc/struct/MatrixStruct.inc Normal file
View File

@@ -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 <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>
wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Unpack(
std::span<const uint8_t> data) {
constexpr size_t kDataOff = 0;
wpi::array<double, Rows * Cols> mat_data =
wpi::UnpackStructArray<double, kDataOff, Rows * Cols>(data);
frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
for (int i = 0; i < Rows * Cols; i++) {
mat(i) = mat_data[i];
}
return mat;
}
template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
requires(Cols != 1)
void wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Pack(
std::span<uint8_t> data,
const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
constexpr size_t kDataOff = 0;
wpi::array<double, Rows * Cols> mat_data(wpi::empty_array);
for (int i = 0; i < Rows * Cols; i++) {
mat_data[i] = value(i);
}
wpi::PackStructArray<kDataOff, Rows * Cols>(data, mat_data);
}

33
wpimath/src/main/native/include/frc/struct/VectorStruct.h Normal file
View File

@@ -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 <fmt/format.h>
#include <wpi/ct_string.h>
#include <wpi/struct/Struct.h>
#include "frc/EigenCore.h"
template <int Size, int Options, int MaxRows, int MaxCols>
struct wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
static constexpr ct_string kTypeName =
wpi::Concat("Vector__"_ct_string, wpi::NumToCtString<Size>());
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<Size>(), "]"_ct_string);
static constexpr std::string_view GetSchema() { return kSchema; }
static frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> Unpack(
std::span<const uint8_t> data);
static void Pack(
std::span<uint8_t> data,
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value);
};
static_assert(wpi::StructSerializable<frc::Vectord<1>>);
static_assert(wpi::StructSerializable<frc::Vectord<3>>);
#include "frc/struct/VectorStruct.inc"

33
wpimath/src/main/native/include/frc/struct/VectorStruct.inc Normal file
View File

@@ -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 <int Size, int Options, int MaxRows, int MaxCols>
frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>
wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Unpack(
std::span<const uint8_t> data) {
constexpr size_t kDataOff = 0;
wpi::array<double, Size> vec_data =
wpi::UnpackStructArray<double, kDataOff, Size>(data);
frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
for (int i = 0; i < Size; i++) {
vec(i) = vec_data[i];
}
return vec;
}
template <int Size, int Options, int MaxRows, int MaxCols>
void wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Pack(
std::span<uint8_t> data,
const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
constexpr size_t kDataOff = 0;
wpi::array<double, Size> vec_data(wpi::empty_array);
for (int i = 0; i < Size; i++) {
vec_data[i] = value(i);
}
wpi::PackStructArray<kDataOff, Size>(data, vec_data);
}

21
wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.h Normal file
View File

@@ -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 <wpi/protobuf/Protobuf.h>
#include "frc/proto/MatrixProto.h"
#include "frc/system/LinearSystem.h"
template <int States, int Inputs, int Outputs>
struct wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>> {
static google::protobuf::Message* New(google::protobuf::Arena* arena);
static frc::LinearSystem<States, Inputs, Outputs> Unpack(
const google::protobuf::Message& msg);
static void Pack(google::protobuf::Message* msg,
const frc::LinearSystem<States, Inputs, Outputs>& value);
};
#include "frc/system/proto/LinearSystemProto.inc"

54
wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.inc Normal file
View File

@@ -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 <stdexcept>
#include <fmt/format.h>
#include <wpi/ProtoHelper.h>
#include "frc/system/proto/LinearSystemProto.h"
#include "system.pb.h"
template <int States, int Inputs, int Outputs>
google::protobuf::Message*
wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufLinearSystem>(arena);
}
template <int States, int Inputs, int Outputs>
frc::LinearSystem<States, Inputs, Outputs>
wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufLinearSystem*>(&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<States, Inputs, Outputs>{
wpi::UnpackProtobuf<frc::Matrixd<States, States>>(m->a()),
wpi::UnpackProtobuf<frc::Matrixd<States, Inputs>>(m->b()),
wpi::UnpackProtobuf<frc::Matrixd<Outputs, States>>(m->c()),
wpi::UnpackProtobuf<frc::Matrixd<Outputs, Inputs>>(m->d())};
}
template <int States, int Inputs, int Outputs>
void wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::Pack(
google::protobuf::Message* msg,
const frc::LinearSystem<States, Inputs, Outputs>& value) {
auto m = static_cast<wpi::proto::ProtobufLinearSystem*>(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());
}

52
wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.h Normal file
View File

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

42
wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.inc Normal file
View File

@@ -0,0 +1,42 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/system/struct/LinearSystemStruct.h"
template <int States, int Inputs, int Outputs>
frc::LinearSystem<States, Inputs, Outputs>
wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
std::span<const uint8_t> data) {
constexpr size_t kAOff = 0;
constexpr size_t kBOff =
kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
constexpr size_t kCOff =
kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
constexpr size_t kDOff =
kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
return frc::LinearSystem<States, Inputs, Outputs>{
wpi::UnpackStruct<frc::Matrixd<States, States>, kAOff>(data),
wpi::UnpackStruct<frc::Matrixd<States, Inputs>, kBOff>(data),
wpi::UnpackStruct<frc::Matrixd<Outputs, States>, kCOff>(data),
wpi::UnpackStruct<frc::Matrixd<Outputs, Inputs>, kDOff>(data)};
}
template <int States, int Inputs, int Outputs>
void wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Pack(
std::span<uint8_t> data,
const frc::LinearSystem<States, Inputs, Outputs>& value) {
constexpr size_t kAOff = 0;
constexpr size_t kBOff =
kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
constexpr size_t kCOff =
kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
constexpr size_t kDOff =
kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
wpi::PackStruct<kAOff>(data, value.A());
wpi::PackStruct<kBOff>(data, value.B());
wpi::PackStruct<kCOff>(data, value.C());
wpi::PackStruct<kDOff>(data, value.D());
}

1
wpimath/src/main/proto/controller.proto
View File

@@ -29,6 +29,7 @@ message ProtobufSimpleMotorFeedforward {
double ks = 1;
double kv = 2;
double ka = 3;
double dt = 4;
}
message ProtobufDifferentialDriveWheelVoltages {

30
wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProtoTest.java Normal file
View File

@@ -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<DifferentialDriveFeedforward, ProtobufDifferentialDriveFeedforward> {
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);
}
}

27
wpimath/src/test/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProtoTest.java Normal file
View File

@@ -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<SimpleMotorFeedforward, ProtobufSimpleMotorFeedforward> {
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());
}
}

28
wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStructTest.java Normal file
View File

@@ -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<DifferentialDriveFeedforward> {
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);
}
}

25
wpimath/src/test/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStructTest.java Normal file
View File

@@ -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<SimpleMotorFeedforward> {
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());
}
}

27
wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProtoTest.java Normal file
View File

@@ -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<MecanumDriveMotorVoltages, ProtobufMecanumDriveMotorVoltages> {
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);
}
}

31
wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProtoTest.java Normal file
View File

@@ -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<SwerveDriveKinematics, ProtobufSwerveDriveKinematics> {
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());
}
}

25
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStructTest.java Normal file
View File

@@ -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<MecanumDriveMotorVoltages> {
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);
}
}

29
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStructTest.java Normal file
View File

@@ -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<SwerveDriveKinematics> {
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());
}
}

29
wpimath/src/test/java/edu/wpi/first/math/proto/MatrixProtoTest.java Normal file
View File

@@ -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<Matrix<N2, N3>, 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<N2, N3> testData, Matrix<N2, N3> data) {
assertEquals(testData, data);
}
}

26
wpimath/src/test/java/edu/wpi/first/math/proto/VectorProtoTest.java Normal file
View File

@@ -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<Vector<N2>, ProtobufVector> {
VectorProtoTest() {
super(VecBuilder.fill(1.1, 1.2), Vector.getProto(Nat.N2()));
}
@Override
public void checkEquals(Vector<N2> testData, Vector<N2> data) {
assertEquals(testData, data);
}
}

33
wpimath/src/test/java/edu/wpi/first/math/spline/proto/CubicHermiteSplineProtoTest.java Normal file
View File

@@ -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<CubicHermiteSpline, ProtobufCubicHermiteSpline> {
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);
}
}

33
wpimath/src/test/java/edu/wpi/first/math/spline/proto/QuinticHermiteSplineProtoTest.java Normal file
View File

@@ -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<QuinticHermiteSpline, ProtobufQuinticHermiteSpline> {
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);
}
}

31
wpimath/src/test/java/edu/wpi/first/math/spline/struct/CubicHermiteSplineStructTest.java Normal file
View File

@@ -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<CubicHermiteSpline> {
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);
}
}

31
wpimath/src/test/java/edu/wpi/first/math/spline/struct/QuinticHermiteSplineStructTest.java Normal file
View File

@@ -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<QuinticHermiteSpline> {
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);
}
}

28
wpimath/src/test/java/edu/wpi/first/math/struct/MatrixStructTest.java Normal file
View File

@@ -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<Matrix<N2, N3>> {
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<N2, N3> testData, Matrix<N2, N3> data) {
assertEquals(testData, data);
}
}

25
wpimath/src/test/java/edu/wpi/first/math/struct/VectorStructTest.java Normal file
View File

@@ -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<Vector<N2>> {
VectorStructTest() {
super(VecBuilder.fill(1.1, 1.2), Vector.getStruct(Nat.N2()));
}
@Override
public void checkEquals(Vector<N2> testData, Vector<N2> data) {
assertEquals(testData, data);
}
}

39
wpimath/src/test/java/edu/wpi/first/math/system/proto/LinearSystemProtoTest.java Normal file
View File

@@ -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<LinearSystem<N2, N3, N4>, 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<N2, N3, N4> testData, LinearSystem<N2, N3, N4> data) {
assertEquals(testData.getA(), data.getA());
assertEquals(testData.getB(), data.getB());
assertEquals(testData.getC(), data.getC());
assertEquals(testData.getD(), data.getD());
}
}

38
wpimath/src/test/java/edu/wpi/first/math/system/struct/LinearSystemStructTest.java Normal file
View File

@@ -0,0 +1,38 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.system.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.wpilibj.StructTestBase;
@SuppressWarnings("PMD.TestClassWithoutTestCases")
class LinearSystemStructTest extends StructTestBase<LinearSystem<N2, N3, N4>> {
LinearSystemStructTest() {
super(
new LinearSystem<>(
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.1, 1.2, 1.3, 1.4),
MatBuilder.fill(Nat.N2(), Nat.N3(), 2.1, 2.2, 2.3, 2.4, 2.5, 2.6),
MatBuilder.fill(Nat.N4(), Nat.N2(), 3.1, 3.2, 3.3, 3.4, 3.5, 3.6, 3.7, 3.8),
MatBuilder.fill(
Nat.N4(), Nat.N3(), 4.01, 4.02, 4.03, 4.04, 4.05, 4.06, 4.07, 4.08, 4.09, 4.10,
4.11, 4.12)),
LinearSystem.getStruct(Nat.N2(), Nat.N3(), Nat.N4()));
}
@Override
public void checkEquals(LinearSystem<N2, N3, N4> testData, LinearSystem<N2, N3, N4> data) {
assertEquals(testData.getA(), data.getA());
assertEquals(testData.getB(), data.getB());
assertEquals(testData.getC(), data.getC());
assertEquals(testData.getD(), data.getD());
}
}

52
wpimath/src/test/java/edu/wpi/first/wpilibj/ProtoTestBase.java Normal file
View File

@@ -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<T, MessageType extends ProtoMessage<MessageType>> {
private final T m_testData;
private final Protobuf<T, MessageType> m_proto;
protected ProtoTestBase(T testData, Protobuf<T, MessageType> 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);
}
}

61
wpimath/src/test/java/edu/wpi/first/wpilibj/StructTestBase.java Normal file
View File

@@ -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<T> {
private final T m_testData;
private final Struct<T> m_struct;
protected StructTestBase(T testData, Struct<T> 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);
}
}

59
wpimath/src/test/native/cpp/ProtoTestBase.h Normal file
View File

@@ -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 <gtest/gtest.h>
#include <wpi/protobuf/Protobuf.h>
#include "controller.pb.h"
#include "kinematics.pb.h"
#include "spline.pb.h"
#include "system.pb.h"
#include "wpimath.pb.h"
template <typename T>
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<Type>::New(&arena);
wpi::PackProtobuf(proto, TypeParam::kTestData);
Type unpacked_data = wpi::UnpackProtobuf<Type>(*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<Type>::New(&arena);
wpi::PackProtobuf(proto, TypeParam::kTestData);
wpi::PackProtobuf(proto, TypeParam::kTestData);
Type unpacked_data = wpi::UnpackProtobuf<Type>(*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<Type>::New(&arena);
wpi::PackProtobuf(proto, TypeParam::kTestData);
{
Type unpacked_data = wpi::UnpackProtobuf<Type>(*proto);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
{
Type unpacked_data = wpi::UnpackProtobuf<Type>(*proto);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
}
REGISTER_TYPED_TEST_SUITE_P(ProtoTest, RoundTrip, DoublePack, DoubleUnpack);

61
wpimath/src/test/native/cpp/StructTestBase.h Normal file
View File

@@ -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 <gtest/gtest.h>
#include <wpi/struct/Struct.h>
template <typename T>
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<Type>;
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
wpi::PackStruct(buffer, TypeParam::kTestData);
Type unpacked_data = wpi::UnpackStruct<Type>(buffer);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
TYPED_TEST_P(StructTest, DoublePack) {
using Type = typename TypeParam::Type;
using StructType = wpi::Struct<Type>;
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<Type>(buffer);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
TYPED_TEST_P(StructTest, DoubleUnpack) {
using Type = typename TypeParam::Type;
using StructType = wpi::Struct<Type>;
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
wpi::PackStruct(buffer, TypeParam::kTestData);
{
Type unpacked_data = wpi::UnpackStruct<Type>(buffer);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
{
Type unpacked_data = wpi::UnpackStruct<Type>(buffer);
TypeParam::CheckEq(TypeParam::kTestData, unpacked_data);
}
}
REGISTER_TYPED_TEST_SUITE_P(StructTest, RoundTrip, DoublePack, DoubleUnpack);

28
wpimath/src/test/native/cpp/controller/proto/DifferentialDriveFeedforwardProtoTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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);

31
wpimath/src/test/native/cpp/controller/proto/SimpleMotorFeedforwardProtoTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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<units::meters>;
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);

28
wpimath/src/test/native/cpp/controller/struct/DifferentialDriveFeedforwardStructTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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);

31
wpimath/src/test/native/cpp/controller/struct/SimpleMotorFeedforwardStructTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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<units::meters>;
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);

26
wpimath/src/test/native/cpp/kinematics/proto/SwerveDriveKinematicsProtoTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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);

26
wpimath/src/test/native/cpp/kinematics/struct/SwerveDriveKinematicsStructTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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);

23
wpimath/src/test/native/cpp/proto/MatrixProtoTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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);

23
wpimath/src/test/native/cpp/proto/VectorProtoTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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);

32
wpimath/src/test/native/cpp/spline/proto/CubicHermiteSplineProtoTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include "../../ProtoTestBase.h"
#include "frc/spline/CubicHermiteSpline.h"
using namespace frc;
struct CubicHermiteSplineProtoTestData {
using Type = CubicHermiteSpline;
inline static const Type kTestData{
wpi::array<double, 2>{{0.1, 0.2}}, wpi::array<double, 2>{{0.3, 0.4}},
wpi::array<double, 2>{{0.5, 0.6}}, wpi::array<double, 2>{{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);

Some files were not shown because too many files have changed in this diff Show More