From 38ef95f78d6bba1306478084ee2d9c0e3290af22 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sun, 17 Aug 2025 12:05:53 +0200 Subject: [PATCH 01/82] feat: add ChassisAccelerations class (similar to ChassisSpeeds but for accelerations) and make both classes interpolatable Signed-off-by: Zach Harel --- .../math/kinematics/ChassisAccelerations.java | 236 ++++++++++++++++++ .../first/math/kinematics/ChassisSpeeds.java | 19 +- 2 files changed, 254 insertions(+), 1 deletion(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java new file mode 100644 index 00000000000..9266b345270 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java @@ -0,0 +1,236 @@ +// 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; + +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +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.interpolation.Interpolatable; +import edu.wpi.first.math.kinematics.struct.ChassisAccelerationsStruct; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.util.struct.StructSerializable; +import java.util.Objects; + +/** + * Represents the acceleration of a robot chassis. Although this class contains similar members + * compared to a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a + * change in pose w.r.t to the robot frame of reference, a ChassisAccelerations object represents a + * robot's velocity. + * + *

A strictly non-holonomic drivetrain, such as a differential drive, should never have an ay + * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum + * will often have all three components. + */ +public class ChassisAccelerations implements StructSerializable, Interpolatable { + /** Velocity along the x-axis in meters per second². (Fwd is +) */ + public double ax; + + /** Velocity along the y-axis in meters per second². (Left is +) */ + public double ay; + + /** Angular velocity of the robot frame in radians per second². (CCW is +) */ + public double alpha; + + /** ChassisAccelerations struct for serialization. */ + public static final ChassisAccelerationsStruct struct = new ChassisAccelerationsStruct(); + + /** Constructs a ChassisAccelerations with zeros for ax, ay, and omega. */ + public ChassisAccelerations() {} + + /** + * Constructs a ChassisAccelerations object. + * + * @param ax Forward acceleration in meters per second². + * @param ay Sideways acceleration in meters per second². + * @param alpha Angular acceleration in radians per second. + */ + public ChassisAccelerations(double ax, double ay, double alpha) { + this.ax = ax; + this.ay = ay; + this.alpha = alpha; + } + + /** + * Constructs a ChassisAccelerations object. + * + * @param ax Forward velocity. + * @param ay Sideways velocity. + * @param alpha Angular velocity. + */ + public ChassisAccelerations( + LinearAcceleration ax, LinearAcceleration ay, AngularAcceleration alpha) { + this( + ax.in(MetersPerSecondPerSecond), + ay.in(MetersPerSecondPerSecond), + alpha.in(RadiansPerSecondPerSecond)); + } + + /** + * Creates a Twist2d from ChassisAccelerations. + * + * @param dt The duration of the timestep in seconds. + * @return Twist2d. + */ + public Twist2d toTwist2d(double dt) { + return new Twist2d(ax * dt, ay * dt, alpha * dt); + } + + /** + * Discretizes a continuous-time chassis speed. + * + *

This function converts this continuous-time chassis speed into a discrete-time one such that + * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + * along the y-axis, and alpha * dt around the z-axis). + * + *

This is useful for compensating for translational skew when translating and rotating a + * holonomic (swerve or mecanum) drivetrain. However, scaling down the ChassisAccelerations after + * discretizing (e.g., when desaturating swerve module speeds) rotates the direction of net motion + * in the opposite direction of rotational velocity, introducing a different translational skew + * which is not accounted for by discretization. + * + * @param dt The duration of the timestep in seconds the speeds should be applied for. + * @return Discretized ChassisAccelerations. + */ + public ChassisAccelerations discretize(double dt) { + // Construct the desired pose after a timestep, relative to the current pose. The desired pose + // has decoupled translation and rotation. + var desiredDeltaPose = new Pose2d(ax * dt, ay * dt, new Rotation2d(alpha * dt)); + + // Find the chassis translation/rotation deltas in the robot frame that move the robot from its + // current pose to the desired pose + var twist = Pose2d.kZero.log(desiredDeltaPose); + + // Turn the chassis translation/rotation deltas into average velocities + return new ChassisAccelerations(twist.dx / dt, twist.dy / dt, twist.dtheta / dt); + } + + /** + * Converts this field-relative set of speeds into a robot-relative ChassisAccelerations object. + * + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is + * considered to be zero when it is facing directly away from your alliance station wall. + * Remember that this should be CCW positive. + * @return ChassisAccelerations object representing the speeds in the robot's frame of reference. + */ + public ChassisAccelerations toRobotRelative(Rotation2d robotAngle) { + // CW rotation into chassis frame + var rotated = new Translation2d(ax, ay).rotateBy(robotAngle.unaryMinus()); + return new ChassisAccelerations(rotated.getX(), rotated.getY(), alpha); + } + + /** + * Converts this robot-relative set of speeds into a field-relative ChassisAccelerations object. + * + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is + * considered to be zero when it is facing directly away from your alliance station wall. + * Remember that this should be CCW positive. + * @return ChassisAccelerations object representing the speeds in the field's frame of reference. + */ + public ChassisAccelerations toFieldRelative(Rotation2d robotAngle) { + // CCW rotation out of chassis frame + var rotated = new Translation2d(ax, ay).rotateBy(robotAngle); + return new ChassisAccelerations(rotated.getX(), rotated.getY(), alpha); + } + + /** + * Adds two ChassisAccelerations and returns the sum. + * + *

For example, ChassisAccelerations{1.0, 0.5, 0.75} + ChassisAccelerations{2.0, 1.5, 0.25} = + * ChassisAccelerations{3.0, 2.0, 1.0} + * + * @param other The ChassisAccelerations to add. + * @return The sum of the ChassisAccelerations. + */ + public ChassisAccelerations plus(ChassisAccelerations other) { + return new ChassisAccelerations(ax + other.ax, ay + other.ay, alpha + other.alpha); + } + + /** + * Subtracts the other ChassisAccelerations from the current ChassisAccelerations and returns the + * difference. + * + *

For example, ChassisAccelerations{5.0, 4.0, 2.0} - ChassisAccelerations{1.0, 2.0, 1.0} = + * ChassisAccelerations{4.0, 2.0, 1.0} + * + * @param other The ChassisAccelerations to subtract. + * @return The difference between the two ChassisAccelerations. + */ + public ChassisAccelerations minus(ChassisAccelerations other) { + return new ChassisAccelerations(ax - other.ax, ay - other.ay, alpha - other.alpha); + } + + /** + * Returns the inverse of the current ChassisAccelerations. This is equivalent to negating all + * components of the ChassisAccelerations. + * + * @return The inverse of the current ChassisAccelerations. + */ + public ChassisAccelerations unaryMinus() { + return new ChassisAccelerations(-ax, -ay, -alpha); + } + + /** + * Multiplies the ChassisAccelerations by a scalar and returns the new ChassisAccelerations. + * + *

For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2 = ChassisAccelerations{4.0, 5.0, 1.0} + * + * @param scalar The scalar to multiply by. + * @return The scaled ChassisAccelerations. + */ + public ChassisAccelerations times(double scalar) { + return new ChassisAccelerations(ax * scalar, ay * scalar, alpha * scalar); + } + + /** + * Divides the ChassisAccelerations by a scalar and returns the new ChassisAccelerations. + * + *

For example, ChassisAccelerations{2.0, 2.5, 1.0} / 2 = ChassisAccelerations{1.0, 1.25, 0.5} + * + * @param scalar The scalar to divide by. + * @return The scaled ChassisAccelerations. + */ + public ChassisAccelerations div(double scalar) { + return new ChassisAccelerations(ax / scalar, ay / scalar, alpha / scalar); + } + + @Override + public ChassisAccelerations interpolate(ChassisAccelerations endValue, double t) { + if (t <= 0) { + return this; + } else if (t >= 1) { + return endValue; + } else { + return new ChassisAccelerations( + MathUtil.interpolate(this.ax, endValue.ax, t), + MathUtil.interpolate(this.ay, endValue.ay, t), + MathUtil.interpolate(this.alpha, endValue.alpha, t) + ); + } + } + + @Override + public final int hashCode() { + return Objects.hash(ax, ay, alpha); + } + + @Override + public boolean equals(Object o) { + return o == this + || o instanceof ChassisAccelerations c && ax == c.ax && ay == c.ay && alpha == c.alpha; + } + + @Override + public String toString() { + return String.format( + "ChassisAccelerations(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)", ax, ay, alpha); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 9de9d528f77..1232fcf5fbb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -7,10 +7,12 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; 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.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto; import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct; import edu.wpi.first.units.measure.AngularVelocity; @@ -28,7 +30,7 @@ * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum * will often have all three components. */ -public class ChassisSpeeds implements ProtobufSerializable, StructSerializable { +public class ChassisSpeeds implements ProtobufSerializable, StructSerializable, Interpolatable { /** Velocity along the x-axis in meters per second. (Fwd is +) */ public double vx; @@ -199,6 +201,21 @@ public ChassisSpeeds div(double scalar) { return new ChassisSpeeds(vx / scalar, vy / scalar, omega / scalar); } + @Override + public ChassisSpeeds interpolate(ChassisSpeeds endValue, double t) { + if (t <= 0) { + return this; + } else if (t >= 1) { + return endValue; + } else { + return new ChassisSpeeds( + MathUtil.interpolate(this.vx, endValue.vx, t), + MathUtil.interpolate(this.vy, endValue.vy, t), + MathUtil.interpolate(this.omega, endValue.omega, t) + ); + } + } + @Override public final int hashCode() { return Objects.hash(vx, vy, omega); From a9d58f9b144e192b60a58c484872b7979b01aa31 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sun, 17 Aug 2025 12:27:43 +0200 Subject: [PATCH 02/82] add struct for accelerations Signed-off-by: Zach Harel --- .../struct/ChassisAccelerationsStruct.java | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisAccelerationsStruct.java diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisAccelerationsStruct.java new file mode 100644 index 00000000000..db9cf940dee --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisAccelerationsStruct.java @@ -0,0 +1,47 @@ +// 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.ChassisAccelerations; +import edu.wpi.first.util.struct.Struct; + +import java.nio.ByteBuffer; + +public class ChassisAccelerationsStruct implements Struct { + @Override + public Class getTypeClass() { + return ChassisAccelerations.class; + } + + @Override + public String getTypeName() { + return "ChassisAccelerations"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double ax;double ay;double alpha"; + } + + @Override + public ChassisAccelerations unpack(ByteBuffer bb) { + double ax = bb.getDouble(); + double ay = bb.getDouble(); + double alpha = bb.getDouble(); + return new ChassisAccelerations(ax, ay, alpha); + } + + @Override + public void pack(ByteBuffer bb, ChassisAccelerations value) { + bb.putDouble(value.ax); + bb.putDouble(value.ay); + bb.putDouble(value.alpha); + } +} From 4a81fc861bee81d4c8dcb787c1eb27b4fae26b4c Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sun, 17 Aug 2025 12:58:54 +0200 Subject: [PATCH 03/82] feat: add protobuf for acceleration Signed-off-by: Zach Harel --- .../edu/wpi/first/math/proto/Kinematics.java | 538 ++++++++++++++++-- .../math/kinematics/ChassisAccelerations.java | 4 + .../proto/ChassisAccelerationsProto.java | 39 ++ wpimath/src/main/proto/kinematics.proto | 6 + 4 files changed, 528 insertions(+), 59 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisAccelerationsProto.java diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java index 76b95da39ce..264e10937c8 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java @@ -19,80 +19,86 @@ import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3020, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3295, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + - "BW9tZWdhIkUKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh4KCnRyYWNrd2lkdGgY" + - "ASABKAFSCnRyYWNrd2lkdGgiUAokUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsU3BlZWRzEhIK" + - "BGxlZnQYASABKAFSBGxlZnQSFAoFcmlnaHQYAiABKAFSBXJpZ2h0IlMKJ1Byb3RvYnVmRGlmZmVyZW50" + - "aWFsRHJpdmVXaGVlbFBvc2l0aW9ucxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVy" + - "aWdodCKkAgoeUHJvdG9idWZNZWNhbnVtRHJpdmVLaW5lbWF0aWNzEj8KCmZyb250X2xlZnQYASABKAsy" + - "IC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglmcm9udExlZnQSQQoLZnJvbnRfcmlnaHQY" + - "AiABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgpmcm9udFJpZ2h0Ej0KCXJlYXJf" + - "bGVmdBgDIAEoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSCHJlYXJMZWZ0Ej8KCnJl" + - "YXJfcmlnaHQYBCABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglyZWFyUmlnaHQi" + - "oAEKIlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxQb3NpdGlvbnMSHQoKZnJvbnRfbGVmdBgBIAEoAVIJ" + - "ZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVmdBgDIAEo" + - "AVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0Ip0BCh9Qcm90b2J1Zk1lY2Fu" + - "dW1Ecml2ZVdoZWVsU3BlZWRzEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9udF9y" + - "aWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJlYXJf" + - "cmlnaHQYBCABKAFSCXJlYXJSaWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURyaXZlS2luZW1hdGljcxI6Cgdt" + - "b2R1bGVzGAEgAygLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIHbW9kdWxlcyJvChxQ" + - "cm90b2J1ZlN3ZXJ2ZU1vZHVsZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVh" + - "bmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVm", - "U3dlcnZlTW9kdWxlU3RhdGUSFAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3Bp" + - "LnByb3RvLlByb3RvYnVmUm90YXRpb24yZFIFYW5nbGVCGgoYZWR1LndwaS5maXJzdC5tYXRoLnByb3Rv" + - "SpkNCgYSBAAAPQEKCAoBDBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAaCggKAQgSAwYAMQoJCgIIARID" + - "BgAxCgoKAgQAEgQIAAwBCgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoMCgUEAAIABRIDCQIICgwKBQQA" + - "AgABEgMJCQsKDAoFBAACAAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAACAQUSAwoCCAoMCgUEAAIBARID" + - "CgkLCgwKBQQAAgEDEgMKDg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgMLAggKDAoFBAACAgESAwsJDgoM" + - "CgUEAAICAxIDCxESCgoKAgQBEgQOABABCgoKAwQBARIDDggrCgsKBAQBAgASAw8CGAoMCgUEAQIABRID" + - "DwIICgwKBQQBAgABEgMPCRMKDAoFBAECAAMSAw8WFwoKCgIEAhIEEgAVAQoKCgMEAgESAxIILAoLCgQE" + - "AgIAEgMTAhIKDAoFBAICAAUSAxMCCAoMCgUEAgIAARIDEwkNCgwKBQQCAgADEgMTEBEKCwoEBAICARID" + - "FAITCgwKBQQCAgEFEgMUAggKDAoFBAICAQESAxQJDgoMCgUEAgIBAxIDFBESCgoKAgQDEgQXABoBCgoK" + - "AwQDARIDFwgvCgsKBAQDAgASAxgCEgoMCgUEAwIABRIDGAIICgwKBQQDAgABEgMYCQ0KDAoFBAMCAAMS" + - "AxgQEQoLCgQEAwIBEgMZAhMKDAoFBAMCAQUSAxkCCAoMCgUEAwIBARIDGQkOCgwKBQQDAgEDEgMZERIK" + - "CgoCBAQSBBwAIQEKCgoDBAQBEgMcCCYKCwoEBAQCABIDHQInCgwKBQQEAgAGEgMdAhcKDAoFBAQCAAES" + - "Ax0YIgoMCgUEBAIAAxIDHSUmCgsKBAQEAgESAx4CKAoMCgUEBAIBBhIDHgIXCgwKBQQEAgEBEgMeGCMK" + - "DAoFBAQCAQMSAx4mJwoLCgQEBAICEgMfAiYKDAoFBAQCAgYSAx8CFwoMCgUEBAICARIDHxghCgwKBQQE" + - "AgIDEgMfJCUKCwoEBAQCAxIDIAInCgwKBQQEAgMGEgMgAhcKDAoFBAQCAwESAyAYIgoMCgUEBAIDAxID" + - "ICUmCgoKAgQFEgQjACgBCgoKAwQFARIDIwgqCgsKBAQFAgASAyQCGAoMCgUEBQIABRIDJAIICgwKBQQF" + - "AgABEgMkCRMKDAoFBAUCAAMSAyQWFwoLCgQEBQIBEgMlAhkKDAoFBAUCAQUSAyUCCAoMCgUEBQIBARID" + - "JQkUCgwKBQQFAgEDEgMlFxgKCwoEBAUCAhIDJgIXCgwKBQQFAgIFEgMmAggKDAoFBAUCAgESAyYJEgoM" + - "CgUEBQICAxIDJhUWCgsKBAQFAgMSAycCGAoMCgUEBQIDBRIDJwIICgwKBQQFAgMBEgMnCRMKDAoFBAUC", - "AwMSAycWFwoKCgIEBhIEKgAvAQoKCgMEBgESAyoIJwoLCgQEBgIAEgMrAhgKDAoFBAYCAAUSAysCCAoM" + - "CgUEBgIAARIDKwkTCgwKBQQGAgADEgMrFhcKCwoEBAYCARIDLAIZCgwKBQQGAgEFEgMsAggKDAoFBAYC" + - "AQESAywJFAoMCgUEBgIBAxIDLBcYCgsKBAQGAgISAy0CFwoMCgUEBgICBRIDLQIICgwKBQQGAgIBEgMt" + - "CRIKDAoFBAYCAgMSAy0VFgoLCgQEBgIDEgMuAhgKDAoFBAYCAwUSAy4CCAoMCgUEBgIDARIDLgkTCgwK" + - "BQQGAgMDEgMuFhcKCgoCBAcSBDEAMwEKCgoDBAcBEgMxCCUKCwoEBAcCABIDMgItCgwKBQQHAgAEEgMy" + - "AgoKDAoFBAcCAAYSAzILIAoMCgUEBwIAARIDMiEoCgwKBQQHAgADEgMyKywKCgoCBAgSBDUAOAEKCgoD" + - "BAgBEgM1CCQKCwoEBAgCABIDNgIWCgwKBQQIAgAFEgM2AggKDAoFBAgCAAESAzYJEQoMCgUECAIAAxID" + - "NhQVCgsKBAQIAgESAzcCHwoMCgUECAIBBhIDNwIUCgwKBQQIAgEBEgM3FRoKDAoFBAgCAQMSAzcdHgoK" + - "CgIECRIEOgA9AQoKCgMECQESAzoIIQoLCgQECQIAEgM7AhMKDAoFBAkCAAUSAzsCCAoMCgUECQIAARID" + - "OwkOCgwKBQQJAgADEgM7ERIKCwoEBAkCARIDPAIfCgwKBQQJAgEGEgM8AhQKDAoFBAkCAQESAzwVGgoM" + - "CgUECQIBAxIDPB0eYgZwcm90bzM="); + "BW9tZWdhIlQKHFByb3RvYnVmQ2hhc3Npc0FjY2VsZXJhdGlvbnMSDgoCYXgYASABKAFSAmF4Eg4KAmF5" + + "GAIgASgBUgJheRIUCgVhbHBoYRgDIAEoAVIFYWxwaGEiRQojUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2" + + "ZUtpbmVtYXRpY3MSHgoKdHJhY2t3aWR0aBgBIAEoAVIKdHJhY2t3aWR0aCJQCiRQcm90b2J1ZkRpZmZl" + + "cmVudGlhbERyaXZlV2hlZWxTcGVlZHMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIF" + + "cmlnaHQiUwonUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsUG9zaXRpb25zEhIKBGxlZnQYASAB" + + "KAFSBGxlZnQSFAoFcmlnaHQYAiABKAFSBXJpZ2h0IqQCCh5Qcm90b2J1Zk1lY2FudW1Ecml2ZUtpbmVt" + + "YXRpY3MSPwoKZnJvbnRfbGVmdBgBIAEoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRS" + + "CWZyb250TGVmdBJBCgtmcm9udF9yaWdodBgCIAEoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0" + + "aW9uMmRSCmZyb250UmlnaHQSPQoJcmVhcl9sZWZ0GAMgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJh" + + "bnNsYXRpb24yZFIIcmVhckxlZnQSPwoKcmVhcl9yaWdodBgEIAEoCzIgLndwaS5wcm90by5Qcm90b2J1" + + "ZlRyYW5zbGF0aW9uMmRSCXJlYXJSaWdodCKgAQoiUHJvdG9idWZNZWNhbnVtRHJpdmVXaGVlbFBvc2l0" + + "aW9ucxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQYAiABKAFSCmZy" + + "b250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0GAQgASgBUgly" + + "ZWFyUmlnaHQinQEKH1Byb3RvYnVmTWVjYW51bURyaXZlV2hlZWxTcGVlZHMSHQoKZnJvbnRfbGVmdBgB" + + "IAEoAVIJZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVm" + + "dBgDIAEoAVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0IlsKHVByb3RvYnVm" + + "U3dlcnZlRHJpdmVLaW5lbWF0aWNzEjoKB21vZHVsZXMYASADKAsyIC53cGkucHJvdG8uUHJvdG9idWZU" + + "cmFuc2xhdGlvbjJkUgdtb2R1bGVzIm8KHFByb3RvYnVmU3dlcnZlTW9kdWxlUG9zaXRpb24SGgoIZGlz", + "dGFuY2UYASABKAFSCGRpc3RhbmNlEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90" + + "YXRpb24yZFIFYW5nbGUiZgoZUHJvdG9idWZTd2VydmVNb2R1bGVTdGF0ZRIUCgVzcGVlZBgBIAEoAVIF" + + "c3BlZWQSMwoFYW5nbGUYAiABKAsyHS53cGkucHJvdG8uUHJvdG9idWZSb3RhdGlvbjJkUgVhbmdsZUIa" + + "ChhlZHUud3BpLmZpcnN0Lm1hdGgucHJvdG9K1g4KBhIEAABDAQoICgEMEgMAABIKCAoBAhIDAgASCgkK" + + "AgMAEgMEABoKCAoBCBIDBgAxCgkKAggBEgMGADEKCgoCBAASBAgADAEKCgoDBAABEgMICB0KCwoEBAAC" + + "ABIDCQIQCgwKBQQAAgAFEgMJAggKDAoFBAACAAESAwkJCwoMCgUEAAIAAxIDCQ4PCgsKBAQAAgESAwoC" + + "EAoMCgUEAAIBBRIDCgIICgwKBQQAAgEBEgMKCQsKDAoFBAACAQMSAwoODwoLCgQEAAICEgMLAhMKDAoF" + + "BAACAgUSAwsCCAoMCgUEAAICARIDCwkOCgwKBQQAAgIDEgMLERIKCgoCBAESBA4AEgEKCgoDBAEBEgMO" + + "CCQKCwoEBAECABIDDwIQCgwKBQQBAgAFEgMPAggKDAoFBAECAAESAw8JCwoMCgUEAQIAAxIDDw4PCgsK" + + "BAQBAgESAxACEAoMCgUEAQIBBRIDEAIICgwKBQQBAgEBEgMQCQsKDAoFBAECAQMSAxAODwoLCgQEAQIC" + + "EgMRAhMKDAoFBAECAgUSAxECCAoMCgUEAQICARIDEQkOCgwKBQQBAgIDEgMRERIKCgoCBAISBBQAFgEK" + + "CgoDBAIBEgMUCCsKCwoEBAICABIDFQIYCgwKBQQCAgAFEgMVAggKDAoFBAICAAESAxUJEwoMCgUEAgIA" + + "AxIDFRYXCgoKAgQDEgQYABsBCgoKAwQDARIDGAgsCgsKBAQDAgASAxkCEgoMCgUEAwIABRIDGQIICgwK" + + "BQQDAgABEgMZCQ0KDAoFBAMCAAMSAxkQEQoLCgQEAwIBEgMaAhMKDAoFBAMCAQUSAxoCCAoMCgUEAwIB" + + "ARIDGgkOCgwKBQQDAgEDEgMaERIKCgoCBAQSBB0AIAEKCgoDBAQBEgMdCC8KCwoEBAQCABIDHgISCgwK" + + "BQQEAgAFEgMeAggKDAoFBAQCAAESAx4JDQoMCgUEBAIAAxIDHhARCgsKBAQEAgESAx8CEwoMCgUEBAIB" + + "BRIDHwIICgwKBQQEAgEBEgMfCQ4KDAoFBAQCAQMSAx8REgoKCgIEBRIEIgAnAQoKCgMEBQESAyIIJgoL" + + "CgQEBQIAEgMjAicKDAoFBAUCAAYSAyMCFwoMCgUEBQIAARIDIxgiCgwKBQQFAgADEgMjJSYKCwoEBAUC" + + "ARIDJAIoCgwKBQQFAgEGEgMkAhcKDAoFBAUCAQESAyQYIwoMCgUEBQIBAxIDJCYnCgsKBAQFAgISAyUC" + + "JgoMCgUEBQICBhIDJQIXCgwKBQQFAgIBEgMlGCEKDAoFBAUCAgMSAyUkJQoLCgQEBQIDEgMmAicKDAoF", + "BAUCAwYSAyYCFwoMCgUEBQIDARIDJhgiCgwKBQQFAgMDEgMmJSYKCgoCBAYSBCkALgEKCgoDBAYBEgMp" + + "CCoKCwoEBAYCABIDKgIYCgwKBQQGAgAFEgMqAggKDAoFBAYCAAESAyoJEwoMCgUEBgIAAxIDKhYXCgsK" + + "BAQGAgESAysCGQoMCgUEBgIBBRIDKwIICgwKBQQGAgEBEgMrCRQKDAoFBAYCAQMSAysXGAoLCgQEBgIC" + + "EgMsAhcKDAoFBAYCAgUSAywCCAoMCgUEBgICARIDLAkSCgwKBQQGAgIDEgMsFRYKCwoEBAYCAxIDLQIY" + + "CgwKBQQGAgMFEgMtAggKDAoFBAYCAwESAy0JEwoMCgUEBgIDAxIDLRYXCgoKAgQHEgQwADUBCgoKAwQH" + + "ARIDMAgnCgsKBAQHAgASAzECGAoMCgUEBwIABRIDMQIICgwKBQQHAgABEgMxCRMKDAoFBAcCAAMSAzEW" + + "FwoLCgQEBwIBEgMyAhkKDAoFBAcCAQUSAzICCAoMCgUEBwIBARIDMgkUCgwKBQQHAgEDEgMyFxgKCwoE" + + "BAcCAhIDMwIXCgwKBQQHAgIFEgMzAggKDAoFBAcCAgESAzMJEgoMCgUEBwICAxIDMxUWCgsKBAQHAgMS" + + "AzQCGAoMCgUEBwIDBRIDNAIICgwKBQQHAgMBEgM0CRMKDAoFBAcCAwMSAzQWFwoKCgIECBIENwA5AQoK" + + "CgMECAESAzcIJQoLCgQECAIAEgM4Ai0KDAoFBAgCAAQSAzgCCgoMCgUECAIABhIDOAsgCgwKBQQIAgAB" + + "EgM4ISgKDAoFBAgCAAMSAzgrLAoKCgIECRIEOwA+AQoKCgMECQESAzsIJAoLCgQECQIAEgM8AhYKDAoF" + + "BAkCAAUSAzwCCAoMCgUECQIAARIDPAkRCgwKBQQJAgADEgM8FBUKCwoEBAkCARIDPQIfCgwKBQQJAgEG" + + "EgM9AhQKDAoFBAkCAQESAz0VGgoMCgUECQIBAxIDPR0eCgoKAgQKEgRAAEMBCgoKAwQKARIDQAghCgsK" + + "BAQKAgASA0ECEwoMCgUECgIABRIDQQIICgwKBQQKAgABEgNBCQ4KDAoFBAoCAAMSA0EREgoLCgQECgIB" + + "EgNCAh8KDAoFBAoCAQYSA0ICFAoMCgUECgIBARIDQhUaCgwKBQQKAgEDEgNCHR5iBnByb3RvMw=="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); static final Descriptors.Descriptor wpi_proto_ProtobufChassisSpeeds_descriptor = descriptor.internalContainedType(49, 77, "ProtobufChassisSpeeds", "wpi.proto.ProtobufChassisSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveKinematics_descriptor = descriptor.internalContainedType(128, 69, "ProtobufDifferentialDriveKinematics", "wpi.proto.ProtobufDifferentialDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufChassisAccelerations_descriptor = descriptor.internalContainedType(128, 84, "ProtobufChassisAccelerations", "wpi.proto.ProtobufChassisAccelerations"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(199, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveKinematics_descriptor = descriptor.internalContainedType(214, 69, "ProtobufDifferentialDriveKinematics", "wpi.proto.ProtobufDifferentialDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(281, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(285, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(367, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(367, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(662, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(453, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(825, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(748, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(984, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(911, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1077, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1070, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1190, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1163, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1276, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); /** * @return this proto file's descriptor. @@ -513,6 +519,420 @@ static class FieldNames { } } + /** + * Protobuf type {@code ProtobufChassisAccelerations} + */ + public static final class ProtobufChassisAccelerations extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double ax = 1; + */ + private double ax; + + /** + * optional double ay = 2; + */ + private double ay; + + /** + * optional double alpha = 3; + */ + private double alpha; + + private ProtobufChassisAccelerations() { + } + + /** + * @return a new empty instance of {@code ProtobufChassisAccelerations} + */ + public static ProtobufChassisAccelerations newInstance() { + return new ProtobufChassisAccelerations(); + } + + /** + * optional double ax = 1; + * @return whether the ax field is set + */ + public boolean hasAx() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double ax = 1; + * @return this + */ + public ProtobufChassisAccelerations clearAx() { + bitField0_ &= ~0x00000001; + ax = 0D; + return this; + } + + /** + * optional double ax = 1; + * @return the ax + */ + public double getAx() { + return ax; + } + + /** + * optional double ax = 1; + * @param value the ax to set + * @return this + */ + public ProtobufChassisAccelerations setAx(final double value) { + bitField0_ |= 0x00000001; + ax = value; + return this; + } + + /** + * optional double ay = 2; + * @return whether the ay field is set + */ + public boolean hasAy() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double ay = 2; + * @return this + */ + public ProtobufChassisAccelerations clearAy() { + bitField0_ &= ~0x00000002; + ay = 0D; + return this; + } + + /** + * optional double ay = 2; + * @return the ay + */ + public double getAy() { + return ay; + } + + /** + * optional double ay = 2; + * @param value the ay to set + * @return this + */ + public ProtobufChassisAccelerations setAy(final double value) { + bitField0_ |= 0x00000002; + ay = value; + return this; + } + + /** + * optional double alpha = 3; + * @return whether the alpha field is set + */ + public boolean hasAlpha() { + return (bitField0_ & 0x00000004) != 0; + } + + /** + * optional double alpha = 3; + * @return this + */ + public ProtobufChassisAccelerations clearAlpha() { + bitField0_ &= ~0x00000004; + alpha = 0D; + return this; + } + + /** + * optional double alpha = 3; + * @return the alpha + */ + public double getAlpha() { + return alpha; + } + + /** + * optional double alpha = 3; + * @param value the alpha to set + * @return this + */ + public ProtobufChassisAccelerations setAlpha(final double value) { + bitField0_ |= 0x00000004; + alpha = value; + return this; + } + + @Override + public ProtobufChassisAccelerations copyFrom(final ProtobufChassisAccelerations other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + ax = other.ax; + ay = other.ay; + alpha = other.alpha; + } + return this; + } + + @Override + public ProtobufChassisAccelerations mergeFrom(final ProtobufChassisAccelerations other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasAx()) { + setAx(other.ax); + } + if (other.hasAy()) { + setAy(other.ay); + } + if (other.hasAlpha()) { + setAlpha(other.alpha); + } + return this; + } + + @Override + public ProtobufChassisAccelerations clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + ax = 0D; + ay = 0D; + alpha = 0D; + return this; + } + + @Override + public ProtobufChassisAccelerations clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufChassisAccelerations)) { + return false; + } + ProtobufChassisAccelerations other = (ProtobufChassisAccelerations) o; + return bitField0_ == other.bitField0_ + && (!hasAx() || ProtoUtil.isEqual(ax, other.ax)) + && (!hasAy() || ProtoUtil.isEqual(ay, other.ay)) + && (!hasAlpha() || ProtoUtil.isEqual(alpha, other.alpha)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 9); + output.writeDoubleNoTag(ax); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(ay); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeRawByte((byte) 25); + output.writeDoubleNoTag(alpha); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000004) != 0) { + size += 9; + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufChassisAccelerations mergeFrom(final ProtoSource input) throws IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 9: { + // ax + ax = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 17) { + break; + } + } + case 17: { + // ay + ay = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 25) { + break; + } + } + case 25: { + // alpha + alpha = input.readDouble(); + bitField0_ |= 0x00000004; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.ax, ax); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.ay, ay); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeDouble(FieldNames.alpha, alpha); + } + output.endObject(); + } + + @Override + public ProtobufChassisAccelerations mergeFrom(final JsonSource input) throws IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 3127: { + if (input.isAtField(FieldNames.ax)) { + if (!input.trySkipNullValue()) { + ax = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case 3128: { + if (input.isAtField(FieldNames.ay)) { + if (!input.trySkipNullValue()) { + ay = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + case 92909918: { + if (input.isAtField(FieldNames.alpha)) { + if (!input.trySkipNullValue()) { + alpha = input.readDouble(); + bitField0_ |= 0x00000004; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufChassisAccelerations clone() { + return new ProtobufChassisAccelerations().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufChassisAccelerations parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufChassisAccelerations(), data).checkInitialized(); + } + + public static ProtobufChassisAccelerations parseFrom(final ProtoSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufChassisAccelerations(), input).checkInitialized(); + } + + public static ProtobufChassisAccelerations parseFrom(final JsonSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufChassisAccelerations(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufChassisAccelerations messages + */ + public static MessageFactory getFactory() { + return ProtobufChassisAccelerationsFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Kinematics.wpi_proto_ProtobufChassisAccelerations_descriptor; + } + + private enum ProtobufChassisAccelerationsFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufChassisAccelerations create() { + return ProtobufChassisAccelerations.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName ax = FieldName.forField("ax"); + + static final FieldName ay = FieldName.forField("ay"); + + static final FieldName alpha = FieldName.forField("alpha"); + } + } + /** * Protobuf type {@code ProtobufDifferentialDriveKinematics} */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java index 9266b345270..7159ce13121 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.struct.ChassisAccelerationsStruct; +import edu.wpi.first.math.kinematics.proto.ChassisAccelerationsProto; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.util.struct.StructSerializable; @@ -42,6 +43,9 @@ public class ChassisAccelerations implements StructSerializable, Interpolatable< /** ChassisAccelerations struct for serialization. */ public static final ChassisAccelerationsStruct struct = new ChassisAccelerationsStruct(); + /** ChassisAccelerations proto for serialization. */ + public static final ChassisAccelerationsProto proto = new ChassisAccelerationsProto(); + /** Constructs a ChassisAccelerations with zeros for ax, ay, and omega. */ public ChassisAccelerations() {} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisAccelerationsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisAccelerationsProto.java new file mode 100644 index 00000000000..236769a6c49 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisAccelerationsProto.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.ChassisAccelerations; +import edu.wpi.first.math.proto.Kinematics.ProtobufChassisAccelerations; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class ChassisAccelerationsProto implements Protobuf { + @Override + public Class getTypeClass() { + return ChassisAccelerations.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufChassisAccelerations.getDescriptor(); + } + + @Override + public ProtobufChassisAccelerations createMessage() { + return ProtobufChassisAccelerations.newInstance(); + } + + @Override + public ChassisAccelerations unpack(ProtobufChassisAccelerations msg) { + return new ChassisAccelerations(msg.getAx(), msg.getAy(), msg.getAlpha()); + } + + @Override + public void pack(ProtobufChassisAccelerations msg, ChassisAccelerations value) { + msg.setAx(value.ax); + msg.setAy(value.ay); + msg.setAlpha(value.alpha); + } +} diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto index 2d25bd4ea4b..c705575898f 100644 --- a/wpimath/src/main/proto/kinematics.proto +++ b/wpimath/src/main/proto/kinematics.proto @@ -12,6 +12,12 @@ message ProtobufChassisSpeeds { double omega = 3; } +message ProtobufChassisAccelerations { + double ax = 1; + double ay = 2; + double alpha = 3; +} + message ProtobufDifferentialDriveKinematics { double trackwidth = 1; } From d959a874c78725a55ba2389ff99ba38710263ab1 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 16:27:40 -0400 Subject: [PATCH 04/82] feat: implement Protobuf serialization for ChassisAccelerations (because it already had the protobuf) and fix toString format Signed-off-by: Zach Harel --- .../first/math/kinematics/ChassisAccelerations.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java index 7159ce13121..bd8e10b7f21 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java @@ -13,10 +13,11 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.kinematics.struct.ChassisAccelerationsStruct; import edu.wpi.first.math.kinematics.proto.ChassisAccelerationsProto; +import edu.wpi.first.math.kinematics.struct.ChassisAccelerationsStruct; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; @@ -30,7 +31,8 @@ * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum * will often have all three components. */ -public class ChassisAccelerations implements StructSerializable, Interpolatable { +public class ChassisAccelerations + implements ProtobufSerializable, StructSerializable, Interpolatable { /** Velocity along the x-axis in meters per second². (Fwd is +) */ public double ax; @@ -216,8 +218,7 @@ public ChassisAccelerations interpolate(ChassisAccelerations endValue, double t) return new ChassisAccelerations( MathUtil.interpolate(this.ax, endValue.ax, t), MathUtil.interpolate(this.ay, endValue.ay, t), - MathUtil.interpolate(this.alpha, endValue.alpha, t) - ); + MathUtil.interpolate(this.alpha, endValue.alpha, t)); } } @@ -235,6 +236,6 @@ public boolean equals(Object o) { @Override public String toString() { return String.format( - "ChassisAccelerations(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)", ax, ay, alpha); + "ChassisAccelerations(Ax: %.2f m/s², Ay: %.2f m/s², Alpha: %.2f rad/s²)", ax, ay, alpha); } } From c502366fa1ae222b96c2fb5d52f30b69e332b78c Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 16:27:46 -0400 Subject: [PATCH 05/82] make spotless happy Signed-off-by: Zach Harel --- .../java/edu/wpi/first/math/kinematics/ChassisSpeeds.java | 6 +++--- .../math/kinematics/proto/ChassisAccelerationsProto.java | 3 ++- .../math/kinematics/struct/ChassisAccelerationsStruct.java | 1 - 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 1232fcf5fbb..009560b745d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -30,7 +30,8 @@ * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum * will often have all three components. */ -public class ChassisSpeeds implements ProtobufSerializable, StructSerializable, Interpolatable { +public class ChassisSpeeds + implements ProtobufSerializable, StructSerializable, Interpolatable { /** Velocity along the x-axis in meters per second. (Fwd is +) */ public double vx; @@ -211,8 +212,7 @@ public ChassisSpeeds interpolate(ChassisSpeeds endValue, double t) { return new ChassisSpeeds( MathUtil.interpolate(this.vx, endValue.vx, t), MathUtil.interpolate(this.vy, endValue.vy, t), - MathUtil.interpolate(this.omega, endValue.omega, t) - ); + MathUtil.interpolate(this.omega, endValue.omega, t)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisAccelerationsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisAccelerationsProto.java index 236769a6c49..42add196560 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisAccelerationsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisAccelerationsProto.java @@ -9,7 +9,8 @@ import edu.wpi.first.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; -public class ChassisAccelerationsProto implements Protobuf { +public class ChassisAccelerationsProto + implements Protobuf { @Override public Class getTypeClass() { return ChassisAccelerations.class; diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisAccelerationsStruct.java index db9cf940dee..c4fa4f6fc6f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisAccelerationsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisAccelerationsStruct.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.kinematics.ChassisAccelerations; import edu.wpi.first.util.struct.Struct; - import java.nio.ByteBuffer; public class ChassisAccelerationsStruct implements Struct { From 2bb10aed72cccf029bb3fd43d9192fc42e98b888 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 17:00:33 -0400 Subject: [PATCH 06/82] add c++ versions of the accelerations class/struct/proto Signed-off-by: Zach Harel --- .../cpp/wpimath/protobuf/kinematics.npb.cpp | 700 +++++++++++------- .../cpp/wpimath/protobuf/kinematics.npb.h | 89 +++ .../proto/ChassisAccelerationsProto.cpp | 31 + .../struct/ChassisAccelerationsStruct.cpp | 34 + .../frc/kinematics/ChassisAccelerations.h | 161 ++++ .../proto/ChassisAccelerationsProto.h | 20 + .../struct/ChassisAccelerationsStruct.h | 24 + 7 files changed, 779 insertions(+), 280 deletions(-) create mode 100644 wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp create mode 100644 wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/proto/ChassisAccelerationsProto.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/struct/ChassisAccelerationsStruct.h diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp index 5010d135e76..b4ac8539c6f 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp @@ -24,297 +24,413 @@ static const uint8_t file_descriptor[] { 0x0a,0x02,0x76,0x79,0x18,0x02,0x20,0x01,0x28,0x01, 0x52,0x02,0x76,0x79,0x12,0x14,0x0a,0x05,0x6f,0x6d, 0x65,0x67,0x61,0x18,0x03,0x20,0x01,0x28,0x01,0x52, -0x05,0x6f,0x6d,0x65,0x67,0x61,0x22,0x45,0x0a,0x23, -0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,0x69, -0x66,0x66,0x65,0x72,0x65,0x6e,0x74,0x69,0x61,0x6c, -0x44,0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,0x65,0x6d, -0x61,0x74,0x69,0x63,0x73,0x12,0x1e,0x0a,0x0a,0x74, -0x72,0x61,0x63,0x6b,0x77,0x69,0x64,0x74,0x68,0x18, -0x01,0x20,0x01,0x28,0x01,0x52,0x0a,0x74,0x72,0x61, -0x63,0x6b,0x77,0x69,0x64,0x74,0x68,0x22,0x50,0x0a, -0x24,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44, -0x69,0x66,0x66,0x65,0x72,0x65,0x6e,0x74,0x69,0x61, -0x6c,0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65, -0x6c,0x53,0x70,0x65,0x65,0x64,0x73,0x12,0x12,0x0a, -0x04,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28, -0x01,0x52,0x04,0x6c,0x65,0x66,0x74,0x12,0x14,0x0a, -0x05,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01, -0x28,0x01,0x52,0x05,0x72,0x69,0x67,0x68,0x74,0x22, -0x53,0x0a,0x27,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, -0x66,0x44,0x69,0x66,0x66,0x65,0x72,0x65,0x6e,0x74, -0x69,0x61,0x6c,0x44,0x72,0x69,0x76,0x65,0x57,0x68, -0x65,0x65,0x6c,0x50,0x6f,0x73,0x69,0x74,0x69,0x6f, +0x05,0x6f,0x6d,0x65,0x67,0x61,0x22,0x54,0x0a,0x1c, +0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x43,0x68, +0x61,0x73,0x73,0x69,0x73,0x41,0x63,0x63,0x65,0x6c, +0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x73,0x12,0x0e, +0x0a,0x02,0x61,0x78,0x18,0x01,0x20,0x01,0x28,0x01, +0x52,0x02,0x61,0x78,0x12,0x0e,0x0a,0x02,0x61,0x79, +0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x02,0x61,0x79, +0x12,0x14,0x0a,0x05,0x61,0x6c,0x70,0x68,0x61,0x18, +0x03,0x20,0x01,0x28,0x01,0x52,0x05,0x61,0x6c,0x70, +0x68,0x61,0x22,0x45,0x0a,0x23,0x50,0x72,0x6f,0x74, +0x6f,0x62,0x75,0x66,0x44,0x69,0x66,0x66,0x65,0x72, +0x65,0x6e,0x74,0x69,0x61,0x6c,0x44,0x72,0x69,0x76, +0x65,0x4b,0x69,0x6e,0x65,0x6d,0x61,0x74,0x69,0x63, +0x73,0x12,0x1e,0x0a,0x0a,0x74,0x72,0x61,0x63,0x6b, +0x77,0x69,0x64,0x74,0x68,0x18,0x01,0x20,0x01,0x28, +0x01,0x52,0x0a,0x74,0x72,0x61,0x63,0x6b,0x77,0x69, +0x64,0x74,0x68,0x22,0x50,0x0a,0x24,0x50,0x72,0x6f, +0x74,0x6f,0x62,0x75,0x66,0x44,0x69,0x66,0x66,0x65, +0x72,0x65,0x6e,0x74,0x69,0x61,0x6c,0x44,0x72,0x69, +0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x53,0x70,0x65, +0x65,0x64,0x73,0x12,0x12,0x0a,0x04,0x6c,0x65,0x66, +0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x04,0x6c, +0x65,0x66,0x74,0x12,0x14,0x0a,0x05,0x72,0x69,0x67, +0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x05, +0x72,0x69,0x67,0x68,0x74,0x22,0x57,0x0a,0x2b,0x50, +0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,0x69,0x66, +0x66,0x65,0x72,0x65,0x6e,0x74,0x69,0x61,0x6c,0x44, +0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x41, +0x63,0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f, 0x6e,0x73,0x12,0x12,0x0a,0x04,0x6c,0x65,0x66,0x74, 0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x04,0x6c,0x65, 0x66,0x74,0x12,0x14,0x0a,0x05,0x72,0x69,0x67,0x68, 0x74,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x05,0x72, -0x69,0x67,0x68,0x74,0x22,0xa4,0x02,0x0a,0x1e,0x50, -0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63, -0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x4b, -0x69,0x6e,0x65,0x6d,0x61,0x74,0x69,0x63,0x73,0x12, -0x3f,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c, -0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x0b,0x32, -0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,0x74, -0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, -0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,0x74,0x69,0x6f, -0x6e,0x32,0x64,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74, -0x4c,0x65,0x66,0x74,0x12,0x41,0x0a,0x0b,0x66,0x72, -0x6f,0x6e,0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18, -0x02,0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70, -0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72, -0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e, -0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52, -0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69,0x67,0x68, -0x74,0x12,0x3d,0x0a,0x09,0x72,0x65,0x61,0x72,0x5f, -0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01,0x28,0x0b, -0x32,0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f, -0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, -0x66,0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,0x74,0x69, -0x6f,0x6e,0x32,0x64,0x52,0x08,0x72,0x65,0x61,0x72, -0x4c,0x65,0x66,0x74,0x12,0x3f,0x0a,0x0a,0x72,0x65, -0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04, +0x69,0x67,0x68,0x74,0x22,0x53,0x0a,0x27,0x50,0x72, +0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,0x69,0x66,0x66, +0x65,0x72,0x65,0x6e,0x74,0x69,0x61,0x6c,0x44,0x72, +0x69,0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x50,0x6f, +0x73,0x69,0x74,0x69,0x6f,0x6e,0x73,0x12,0x12,0x0a, +0x04,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28, +0x01,0x52,0x04,0x6c,0x65,0x66,0x74,0x12,0x14,0x0a, +0x05,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01, +0x28,0x01,0x52,0x05,0x72,0x69,0x67,0x68,0x74,0x22, +0xa4,0x02,0x0a,0x1e,0x50,0x72,0x6f,0x74,0x6f,0x62, +0x75,0x66,0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44, +0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,0x65,0x6d,0x61, +0x74,0x69,0x63,0x73,0x12,0x3f,0x0a,0x0a,0x66,0x72, +0x6f,0x6e,0x74,0x5f,0x6c,0x65,0x66,0x74,0x18,0x01, 0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69, 0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f, 0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73, 0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x09, -0x72,0x65,0x61,0x72,0x52,0x69,0x67,0x68,0x74,0x22, -0xa0,0x01,0x0a,0x22,0x50,0x72,0x6f,0x74,0x6f,0x62, -0x75,0x66,0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44, -0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x50, -0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x73,0x12,0x1d, -0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c,0x65, -0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x09, 0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74,0x12, -0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x72, -0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x01, -0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69,0x67, -0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,0x61,0x72, -0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01,0x28, -0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,0x65,0x66, -0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,0x72,0x5f, -0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,0x01,0x28, -0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69,0x67, -0x68,0x74,0x22,0x9d,0x01,0x0a,0x1f,0x50,0x72,0x6f, -0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63,0x61,0x6e, -0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65, -0x65,0x6c,0x53,0x70,0x65,0x65,0x64,0x73,0x12,0x1d, -0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c,0x65, -0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x09, -0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74,0x12, -0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x72, -0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x01, -0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69,0x67, -0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,0x61,0x72, -0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01,0x28, -0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,0x65,0x66, -0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,0x72,0x5f, -0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,0x01,0x28, -0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69,0x67, -0x68,0x74,0x22,0x5b,0x0a,0x1d,0x50,0x72,0x6f,0x74, -0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72,0x76,0x65, -0x44,0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,0x65,0x6d, -0x61,0x74,0x69,0x63,0x73,0x12,0x3a,0x0a,0x07,0x6d, -0x6f,0x64,0x75,0x6c,0x65,0x73,0x18,0x01,0x20,0x03, -0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69,0x2e,0x70, -0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f, -0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73,0x6c,0x61, -0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x07,0x6d,0x6f, -0x64,0x75,0x6c,0x65,0x73,0x22,0x6f,0x0a,0x1c,0x50, -0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65, -0x72,0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65,0x50, -0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x12,0x1a,0x0a, -0x08,0x64,0x69,0x73,0x74,0x61,0x6e,0x63,0x65,0x18, -0x01,0x20,0x01,0x28,0x01,0x52,0x08,0x64,0x69,0x73, -0x74,0x61,0x6e,0x63,0x65,0x12,0x33,0x0a,0x05,0x61, -0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01,0x28,0x0b, -0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f, +0x41,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x72, +0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x0b, +0x32,0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f, 0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, -0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f,0x6e,0x32, -0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65,0x22,0x66, -0x0a,0x19,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, -0x53,0x77,0x65,0x72,0x76,0x65,0x4d,0x6f,0x64,0x75, -0x6c,0x65,0x53,0x74,0x61,0x74,0x65,0x12,0x14,0x0a, -0x05,0x73,0x70,0x65,0x65,0x64,0x18,0x01,0x20,0x01, -0x28,0x01,0x52,0x05,0x73,0x70,0x65,0x65,0x64,0x12, -0x33,0x0a,0x05,0x61,0x6e,0x67,0x6c,0x65,0x18,0x02, -0x20,0x01,0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69, +0x66,0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,0x74,0x69, +0x6f,0x6e,0x32,0x64,0x52,0x0a,0x66,0x72,0x6f,0x6e, +0x74,0x52,0x69,0x67,0x68,0x74,0x12,0x3d,0x0a,0x09, +0x72,0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,0x18, +0x03,0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70, +0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72, +0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e, +0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52, +0x08,0x72,0x65,0x61,0x72,0x4c,0x65,0x66,0x74,0x12, +0x3f,0x0a,0x0a,0x72,0x65,0x61,0x72,0x5f,0x72,0x69, +0x67,0x68,0x74,0x18,0x04,0x20,0x01,0x28,0x0b,0x32, +0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,0x74, +0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, +0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,0x74,0x69,0x6f, +0x6e,0x32,0x64,0x52,0x09,0x72,0x65,0x61,0x72,0x52, +0x69,0x67,0x68,0x74,0x22,0xa0,0x01,0x0a,0x22,0x50, +0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63, +0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x57, +0x68,0x65,0x65,0x6c,0x50,0x6f,0x73,0x69,0x74,0x69, +0x6f,0x6e,0x73,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f, +0x6e,0x74,0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,0x20, +0x01,0x28,0x01,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74, +0x4c,0x65,0x66,0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72, +0x6f,0x6e,0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18, +0x02,0x20,0x01,0x28,0x01,0x52,0x0a,0x66,0x72,0x6f, +0x6e,0x74,0x52,0x69,0x67,0x68,0x74,0x12,0x1b,0x0a, +0x09,0x72,0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74, +0x18,0x03,0x20,0x01,0x28,0x01,0x52,0x08,0x72,0x65, +0x61,0x72,0x4c,0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a, +0x72,0x65,0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74, +0x18,0x04,0x20,0x01,0x28,0x01,0x52,0x09,0x72,0x65, +0x61,0x72,0x52,0x69,0x67,0x68,0x74,0x22,0x9d,0x01, +0x0a,0x1f,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, +0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44,0x72,0x69, +0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x53,0x70,0x65, +0x65,0x64,0x73,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f, +0x6e,0x74,0x5f,0x6c,0x65,0x66,0x74,0x18,0x01,0x20, +0x01,0x28,0x01,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74, +0x4c,0x65,0x66,0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72, +0x6f,0x6e,0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18, +0x02,0x20,0x01,0x28,0x01,0x52,0x0a,0x66,0x72,0x6f, +0x6e,0x74,0x52,0x69,0x67,0x68,0x74,0x12,0x1b,0x0a, +0x09,0x72,0x65,0x61,0x72,0x5f,0x6c,0x65,0x66,0x74, +0x18,0x03,0x20,0x01,0x28,0x01,0x52,0x08,0x72,0x65, +0x61,0x72,0x4c,0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a, +0x72,0x65,0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74, +0x18,0x04,0x20,0x01,0x28,0x01,0x52,0x09,0x72,0x65, +0x61,0x72,0x52,0x69,0x67,0x68,0x74,0x22,0xa4,0x01, +0x0a,0x26,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, +0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44,0x72,0x69, +0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x41,0x63,0x63, +0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x73, +0x12,0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f, +0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01, +0x52,0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66, +0x74,0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74, +0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01, +0x28,0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52, +0x69,0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65, +0x61,0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20, +0x01,0x28,0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c, +0x65,0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61, +0x72,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20, +0x01,0x28,0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52, +0x69,0x67,0x68,0x74,0x22,0x5b,0x0a,0x1d,0x50,0x72, +0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72, +0x76,0x65,0x44,0x72,0x69,0x76,0x65,0x4b,0x69,0x6e, +0x65,0x6d,0x61,0x74,0x69,0x63,0x73,0x12,0x3a,0x0a, +0x07,0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,0x18,0x01, +0x20,0x03,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69, 0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f, -0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74,0x61,0x74, -0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61,0x6e,0x67, -0x6c,0x65,0x42,0x1a,0x0a,0x18,0x65,0x64,0x75,0x2e, -0x77,0x70,0x69,0x2e,0x66,0x69,0x72,0x73,0x74,0x2e, -0x6d,0x61,0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f, -0x4a,0x99,0x0d,0x0a,0x06,0x12,0x04,0x00,0x00,0x3d, -0x01,0x0a,0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00, -0x12,0x0a,0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00, -0x12,0x0a,0x09,0x0a,0x02,0x03,0x00,0x12,0x03,0x04, -0x00,0x1a,0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x06, -0x00,0x31,0x0a,0x09,0x0a,0x02,0x08,0x01,0x12,0x03, -0x06,0x00,0x31,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12, -0x04,0x08,0x00,0x0c,0x01,0x0a,0x0a,0x0a,0x03,0x04, -0x00,0x01,0x12,0x03,0x08,0x08,0x1d,0x0a,0x0b,0x0a, -0x04,0x04,0x00,0x02,0x00,0x12,0x03,0x09,0x02,0x10, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12, -0x03,0x09,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x00,0x01,0x12,0x03,0x09,0x09,0x0b,0x0a,0x0c, -0x0a,0x05,0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x09, -0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01, -0x12,0x03,0x0a,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x01,0x05,0x12,0x03,0x0a,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03, -0x0a,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x01,0x03,0x12,0x03,0x0a,0x0e,0x0f,0x0a,0x0b,0x0a, -0x04,0x04,0x00,0x02,0x02,0x12,0x03,0x0b,0x02,0x13, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12, -0x03,0x0b,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x02,0x01,0x12,0x03,0x0b,0x09,0x0e,0x0a,0x0c, -0x0a,0x05,0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0b, -0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12,0x04, -0x0e,0x00,0x10,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01, -0x01,0x12,0x03,0x0e,0x08,0x2b,0x0a,0x0b,0x0a,0x04, -0x04,0x01,0x02,0x00,0x12,0x03,0x0f,0x02,0x18,0x0a, -0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x05,0x12,0x03, -0x0f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, -0x00,0x01,0x12,0x03,0x0f,0x09,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x01,0x02,0x00,0x03,0x12,0x03,0x0f,0x16, -0x17,0x0a,0x0a,0x0a,0x02,0x04,0x02,0x12,0x04,0x12, -0x00,0x15,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01, -0x12,0x03,0x12,0x08,0x2c,0x0a,0x0b,0x0a,0x04,0x04, -0x02,0x02,0x00,0x12,0x03,0x13,0x02,0x12,0x0a,0x0c, -0x0a,0x05,0x04,0x02,0x02,0x00,0x05,0x12,0x03,0x13, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00, -0x01,0x12,0x03,0x13,0x09,0x0d,0x0a,0x0c,0x0a,0x05, -0x04,0x02,0x02,0x00,0x03,0x12,0x03,0x13,0x10,0x11, -0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x01,0x12,0x03, -0x14,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, -0x01,0x05,0x12,0x03,0x14,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x02,0x02,0x01,0x01,0x12,0x03,0x14,0x09, -0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x01,0x03, -0x12,0x03,0x14,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04, -0x03,0x12,0x04,0x17,0x00,0x1a,0x01,0x0a,0x0a,0x0a, -0x03,0x04,0x03,0x01,0x12,0x03,0x17,0x08,0x2f,0x0a, -0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12,0x03,0x18, -0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00, -0x05,0x12,0x03,0x18,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x18,0x09,0x0d, -0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x03,0x12, -0x03,0x18,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x03, -0x02,0x01,0x12,0x03,0x19,0x02,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03,0x19,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x01, -0x12,0x03,0x19,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, -0x03,0x02,0x01,0x03,0x12,0x03,0x19,0x11,0x12,0x0a, -0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1c,0x00,0x21, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01,0x12,0x03, -0x1c,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02, -0x00,0x12,0x03,0x1d,0x02,0x27,0x0a,0x0c,0x0a,0x05, -0x04,0x04,0x02,0x00,0x06,0x12,0x03,0x1d,0x02,0x17, -0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x01,0x12, -0x03,0x1d,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,0x04, -0x02,0x00,0x03,0x12,0x03,0x1d,0x25,0x26,0x0a,0x0b, -0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03,0x1e,0x02, -0x28,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x06, -0x12,0x03,0x1e,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, -0x04,0x02,0x01,0x01,0x12,0x03,0x1e,0x18,0x23,0x0a, -0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03,0x12,0x03, -0x1e,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02, -0x02,0x12,0x03,0x1f,0x02,0x26,0x0a,0x0c,0x0a,0x05, -0x04,0x04,0x02,0x02,0x06,0x12,0x03,0x1f,0x02,0x17, -0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x02,0x01,0x12, -0x03,0x1f,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x04, -0x02,0x02,0x03,0x12,0x03,0x1f,0x24,0x25,0x0a,0x0b, -0x0a,0x04,0x04,0x04,0x02,0x03,0x12,0x03,0x20,0x02, -0x27,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x03,0x06, -0x12,0x03,0x20,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, -0x04,0x02,0x03,0x01,0x12,0x03,0x20,0x18,0x22,0x0a, -0x0c,0x0a,0x05,0x04,0x04,0x02,0x03,0x03,0x12,0x03, -0x20,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x05,0x12, -0x04,0x23,0x00,0x28,0x01,0x0a,0x0a,0x0a,0x03,0x04, -0x05,0x01,0x12,0x03,0x23,0x08,0x2a,0x0a,0x0b,0x0a, -0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x24,0x02,0x18, -0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x05,0x12, -0x03,0x24,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05, -0x02,0x00,0x01,0x12,0x03,0x24,0x09,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03,0x24, -0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x01, -0x12,0x03,0x25,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04, -0x05,0x02,0x01,0x05,0x12,0x03,0x25,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01,0x12,0x03, -0x25,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, -0x01,0x03,0x12,0x03,0x25,0x17,0x18,0x0a,0x0b,0x0a, -0x04,0x04,0x05,0x02,0x02,0x12,0x03,0x26,0x02,0x17, -0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x02,0x05,0x12, -0x03,0x26,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05, -0x02,0x02,0x01,0x12,0x03,0x26,0x09,0x12,0x0a,0x0c, -0x0a,0x05,0x04,0x05,0x02,0x02,0x03,0x12,0x03,0x26, -0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x03, -0x12,0x03,0x27,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, -0x05,0x02,0x03,0x05,0x12,0x03,0x27,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x05,0x02,0x03,0x01,0x12,0x03, -0x27,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, -0x03,0x03,0x12,0x03,0x27,0x16,0x17,0x0a,0x0a,0x0a, -0x02,0x04,0x06,0x12,0x04,0x2a,0x00,0x2f,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03,0x2a,0x08, -0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x00,0x12, -0x03,0x2b,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x00,0x05,0x12,0x03,0x2b,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12,0x03,0x2b, -0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00, -0x03,0x12,0x03,0x2b,0x16,0x17,0x0a,0x0b,0x0a,0x04, -0x04,0x06,0x02,0x01,0x12,0x03,0x2c,0x02,0x19,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x05,0x12,0x03, -0x2c,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, -0x01,0x01,0x12,0x03,0x2c,0x09,0x14,0x0a,0x0c,0x0a, -0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x2c,0x17, -0x18,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x02,0x12, -0x03,0x2d,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x02,0x05,0x12,0x03,0x2d,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03,0x2d, -0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02, -0x03,0x12,0x03,0x2d,0x15,0x16,0x0a,0x0b,0x0a,0x04, -0x04,0x06,0x02,0x03,0x12,0x03,0x2e,0x02,0x18,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x05,0x12,0x03, -0x2e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, -0x03,0x01,0x12,0x03,0x2e,0x09,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03,0x2e,0x16, -0x17,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12,0x04,0x31, -0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x07,0x01, -0x12,0x03,0x31,0x08,0x25,0x0a,0x0b,0x0a,0x04,0x04, -0x07,0x02,0x00,0x12,0x03,0x32,0x02,0x2d,0x0a,0x0c, -0x0a,0x05,0x04,0x07,0x02,0x00,0x04,0x12,0x03,0x32, -0x02,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00, -0x06,0x12,0x03,0x32,0x0b,0x20,0x0a,0x0c,0x0a,0x05, -0x04,0x07,0x02,0x00,0x01,0x12,0x03,0x32,0x21,0x28, -0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x03,0x12, -0x03,0x32,0x2b,0x2c,0x0a,0x0a,0x0a,0x02,0x04,0x08, -0x12,0x04,0x35,0x00,0x38,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x08,0x01,0x12,0x03,0x35,0x08,0x24,0x0a,0x0b, -0x0a,0x04,0x04,0x08,0x02,0x00,0x12,0x03,0x36,0x02, -0x16,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x05, -0x12,0x03,0x36,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x08,0x02,0x00,0x01,0x12,0x03,0x36,0x09,0x11,0x0a, -0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x03,0x12,0x03, -0x36,0x14,0x15,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02, -0x01,0x12,0x03,0x37,0x02,0x1f,0x0a,0x0c,0x0a,0x05, -0x04,0x08,0x02,0x01,0x06,0x12,0x03,0x37,0x02,0x14, -0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x01,0x12, -0x03,0x37,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x08, -0x02,0x01,0x03,0x12,0x03,0x37,0x1d,0x1e,0x0a,0x0a, -0x0a,0x02,0x04,0x09,0x12,0x04,0x3a,0x00,0x3d,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01,0x12,0x03,0x3a, -0x08,0x21,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x00, -0x12,0x03,0x3b,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04, -0x09,0x02,0x00,0x05,0x12,0x03,0x3b,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x01,0x12,0x03, -0x3b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, -0x00,0x03,0x12,0x03,0x3b,0x11,0x12,0x0a,0x0b,0x0a, -0x04,0x04,0x09,0x02,0x01,0x12,0x03,0x3c,0x02,0x1f, -0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x06,0x12, -0x03,0x3c,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09, -0x02,0x01,0x01,0x12,0x03,0x3c,0x15,0x1a,0x0a,0x0c, -0x0a,0x05,0x04,0x09,0x02,0x01,0x03,0x12,0x03,0x3c, -0x1d,0x1e,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, - +0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73, +0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x07, +0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,0x22,0x6f,0x0a, +0x1c,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x53, +0x77,0x65,0x72,0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c, +0x65,0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x12, +0x1a,0x0a,0x08,0x64,0x69,0x73,0x74,0x61,0x6e,0x63, +0x65,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x08,0x64, +0x69,0x73,0x74,0x61,0x6e,0x63,0x65,0x12,0x33,0x0a, +0x05,0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01, +0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70, +0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f, +0x62,0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f, +0x6e,0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65, +0x22,0x66,0x0a,0x19,0x50,0x72,0x6f,0x74,0x6f,0x62, +0x75,0x66,0x53,0x77,0x65,0x72,0x76,0x65,0x4d,0x6f, +0x64,0x75,0x6c,0x65,0x53,0x74,0x61,0x74,0x65,0x12, +0x14,0x0a,0x05,0x73,0x70,0x65,0x65,0x64,0x18,0x01, +0x20,0x01,0x28,0x01,0x52,0x05,0x73,0x70,0x65,0x65, +0x64,0x12,0x33,0x0a,0x05,0x61,0x6e,0x67,0x6c,0x65, +0x18,0x02,0x20,0x01,0x28,0x0b,0x32,0x1d,0x2e,0x77, +0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50, +0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74, +0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61, +0x6e,0x67,0x6c,0x65,0x22,0x7a,0x0a,0x21,0x50,0x72, +0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72, +0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65,0x41,0x63, +0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,0x6e, +0x73,0x12,0x22,0x0a,0x0c,0x61,0x63,0x63,0x65,0x6c, +0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x18,0x01,0x20, +0x01,0x28,0x01,0x52,0x0c,0x61,0x63,0x63,0x65,0x6c, +0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x12,0x31,0x0a, +0x14,0x61,0x6e,0x67,0x75,0x6c,0x61,0x72,0x5f,0x61, +0x63,0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f, +0x6e,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x13,0x61, +0x6e,0x67,0x75,0x6c,0x61,0x72,0x41,0x63,0x63,0x65, +0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x42,0x1a, +0x0a,0x18,0x65,0x64,0x75,0x2e,0x77,0x70,0x69,0x2e, +0x66,0x69,0x72,0x73,0x74,0x2e,0x6d,0x61,0x74,0x68, +0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0xd6,0x12,0x0a, +0x06,0x12,0x04,0x00,0x00,0x54,0x01,0x0a,0x08,0x0a, +0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08,0x0a, +0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x09,0x0a, +0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x1a,0x0a,0x08, +0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x31,0x0a,0x09, +0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00,0x31,0x0a, +0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x08,0x00,0x0c, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12,0x03, +0x08,0x08,0x1d,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02, +0x00,0x12,0x03,0x09,0x02,0x10,0x0a,0x0c,0x0a,0x05, +0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x09,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01,0x12, +0x03,0x09,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00, +0x02,0x00,0x03,0x12,0x03,0x09,0x0e,0x0f,0x0a,0x0b, +0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x0a,0x02, +0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x05, +0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x01,0x01,0x12,0x03,0x0a,0x09,0x0b,0x0a, +0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12,0x03, +0x0a,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02, +0x02,0x12,0x03,0x0b,0x02,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x0b,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01,0x12, +0x03,0x0b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x00, +0x02,0x02,0x03,0x12,0x03,0x0b,0x11,0x12,0x0a,0x0a, +0x0a,0x02,0x04,0x01,0x12,0x04,0x0e,0x00,0x12,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,0x03,0x0e, +0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x00, +0x12,0x03,0x0f,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04, +0x01,0x02,0x00,0x05,0x12,0x03,0x0f,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x01,0x12,0x03, +0x0f,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, +0x00,0x03,0x12,0x03,0x0f,0x0e,0x0f,0x0a,0x0b,0x0a, +0x04,0x04,0x01,0x02,0x01,0x12,0x03,0x10,0x02,0x10, +0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x05,0x12, +0x03,0x10,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01, +0x02,0x01,0x01,0x12,0x03,0x10,0x09,0x0b,0x0a,0x0c, +0x0a,0x05,0x04,0x01,0x02,0x01,0x03,0x12,0x03,0x10, +0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x02, +0x12,0x03,0x11,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04, +0x01,0x02,0x02,0x05,0x12,0x03,0x11,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x01,0x12,0x03, +0x11,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, +0x02,0x03,0x12,0x03,0x11,0x11,0x12,0x0a,0x0a,0x0a, +0x02,0x04,0x02,0x12,0x04,0x14,0x00,0x16,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x02,0x01,0x12,0x03,0x14,0x08, +0x2b,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x00,0x12, +0x03,0x15,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x02, +0x02,0x00,0x05,0x12,0x03,0x15,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x02,0x02,0x00,0x01,0x12,0x03,0x15, +0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00, +0x03,0x12,0x03,0x15,0x16,0x17,0x0a,0x0a,0x0a,0x02, +0x04,0x03,0x12,0x04,0x18,0x00,0x1b,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x18,0x08,0x2c, +0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12,0x03, +0x19,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, +0x00,0x05,0x12,0x03,0x19,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x19,0x09, +0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x03, +0x12,0x03,0x19,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04, +0x03,0x02,0x01,0x12,0x03,0x1a,0x02,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03,0x1a, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01, +0x01,0x12,0x03,0x1a,0x09,0x0e,0x0a,0x0c,0x0a,0x05, +0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x1a,0x11,0x12, +0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1d,0x00, +0x20,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01,0x12, +0x03,0x1d,0x08,0x33,0x0a,0x0b,0x0a,0x04,0x04,0x04, +0x02,0x00,0x12,0x03,0x1e,0x02,0x12,0x0a,0x0c,0x0a, +0x05,0x04,0x04,0x02,0x00,0x05,0x12,0x03,0x1e,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x01, +0x12,0x03,0x1e,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04, +0x04,0x02,0x00,0x03,0x12,0x03,0x1e,0x10,0x11,0x0a, +0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03,0x1f, +0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01, +0x05,0x12,0x03,0x1f,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x1f,0x09,0x0e, +0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03,0x12, +0x03,0x1f,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x05, +0x12,0x04,0x22,0x00,0x25,0x01,0x0a,0x0a,0x0a,0x03, +0x04,0x05,0x01,0x12,0x03,0x22,0x08,0x2f,0x0a,0x0b, +0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x23,0x02, +0x12,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x05, +0x12,0x03,0x23,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x05,0x02,0x00,0x01,0x12,0x03,0x23,0x09,0x0d,0x0a, +0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03, +0x23,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02, +0x01,0x12,0x03,0x24,0x02,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x24,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01,0x12, +0x03,0x24,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x05, +0x02,0x01,0x03,0x12,0x03,0x24,0x11,0x12,0x0a,0x0a, +0x0a,0x02,0x04,0x06,0x12,0x04,0x27,0x00,0x2c,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03,0x27, +0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x00, +0x12,0x03,0x28,0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04, +0x06,0x02,0x00,0x06,0x12,0x03,0x28,0x02,0x17,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12,0x03, +0x28,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x00,0x03,0x12,0x03,0x28,0x25,0x26,0x0a,0x0b,0x0a, +0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x29,0x02,0x28, +0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x06,0x12, +0x03,0x29,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x01,0x01,0x12,0x03,0x29,0x18,0x23,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x29, +0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x02, +0x12,0x03,0x2a,0x02,0x26,0x0a,0x0c,0x0a,0x05,0x04, +0x06,0x02,0x02,0x06,0x12,0x03,0x2a,0x02,0x17,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03, +0x2a,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x02,0x03,0x12,0x03,0x2a,0x24,0x25,0x0a,0x0b,0x0a, +0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2b,0x02,0x27, +0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x06,0x12, +0x03,0x2b,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x03,0x01,0x12,0x03,0x2b,0x18,0x22,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03,0x2b, +0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12,0x04, +0x2e,0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x07, +0x01,0x12,0x03,0x2e,0x08,0x2a,0x0a,0x0b,0x0a,0x04, +0x04,0x07,0x02,0x00,0x12,0x03,0x2f,0x02,0x18,0x0a, +0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x05,0x12,0x03, +0x2f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, +0x00,0x01,0x12,0x03,0x2f,0x09,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x07,0x02,0x00,0x03,0x12,0x03,0x2f,0x16, +0x17,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x01,0x12, +0x03,0x30,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x07, +0x02,0x01,0x05,0x12,0x03,0x30,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x07,0x02,0x01,0x01,0x12,0x03,0x30, +0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01, +0x03,0x12,0x03,0x30,0x17,0x18,0x0a,0x0b,0x0a,0x04, +0x04,0x07,0x02,0x02,0x12,0x03,0x31,0x02,0x17,0x0a, +0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x05,0x12,0x03, +0x31,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, +0x02,0x01,0x12,0x03,0x31,0x09,0x12,0x0a,0x0c,0x0a, +0x05,0x04,0x07,0x02,0x02,0x03,0x12,0x03,0x31,0x15, +0x16,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x03,0x12, +0x03,0x32,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07, +0x02,0x03,0x05,0x12,0x03,0x32,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x07,0x02,0x03,0x01,0x12,0x03,0x32, +0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03, +0x03,0x12,0x03,0x32,0x16,0x17,0x0a,0x0a,0x0a,0x02, +0x04,0x08,0x12,0x04,0x35,0x00,0x3a,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x08,0x01,0x12,0x03,0x35,0x08,0x27, +0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x00,0x12,0x03, +0x36,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, +0x00,0x05,0x12,0x03,0x36,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x08,0x02,0x00,0x01,0x12,0x03,0x36,0x09, +0x13,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x03, +0x12,0x03,0x36,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04, +0x08,0x02,0x01,0x12,0x03,0x37,0x02,0x19,0x0a,0x0c, +0x0a,0x05,0x04,0x08,0x02,0x01,0x05,0x12,0x03,0x37, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01, +0x01,0x12,0x03,0x37,0x09,0x14,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x01,0x03,0x12,0x03,0x37,0x17,0x18, +0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x02,0x12,0x03, +0x38,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, +0x02,0x05,0x12,0x03,0x38,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x08,0x02,0x02,0x01,0x12,0x03,0x38,0x09, +0x12,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x03, +0x12,0x03,0x38,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04, +0x08,0x02,0x03,0x12,0x03,0x39,0x02,0x18,0x0a,0x0c, +0x0a,0x05,0x04,0x08,0x02,0x03,0x05,0x12,0x03,0x39, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03, +0x01,0x12,0x03,0x39,0x09,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x03,0x03,0x12,0x03,0x39,0x16,0x17, +0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,0x3c,0x00, +0x41,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01,0x12, +0x03,0x3c,0x08,0x2e,0x0a,0x0b,0x0a,0x04,0x04,0x09, +0x02,0x00,0x12,0x03,0x3d,0x02,0x18,0x0a,0x0c,0x0a, +0x05,0x04,0x09,0x02,0x00,0x05,0x12,0x03,0x3d,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x01, +0x12,0x03,0x3d,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x00,0x03,0x12,0x03,0x3d,0x16,0x17,0x0a, +0x0b,0x0a,0x04,0x04,0x09,0x02,0x01,0x12,0x03,0x3e, +0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01, +0x05,0x12,0x03,0x3e,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x09,0x02,0x01,0x01,0x12,0x03,0x3e,0x09,0x14, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x03,0x12, +0x03,0x3e,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x09, +0x02,0x02,0x12,0x03,0x3f,0x02,0x17,0x0a,0x0c,0x0a, +0x05,0x04,0x09,0x02,0x02,0x05,0x12,0x03,0x3f,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x01, +0x12,0x03,0x3f,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x02,0x03,0x12,0x03,0x3f,0x15,0x16,0x0a, +0x0b,0x0a,0x04,0x04,0x09,0x02,0x03,0x12,0x03,0x40, +0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03, +0x05,0x12,0x03,0x40,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x09,0x02,0x03,0x01,0x12,0x03,0x40,0x09,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x03,0x12, +0x03,0x40,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x0a, +0x12,0x04,0x43,0x00,0x45,0x01,0x0a,0x0a,0x0a,0x03, +0x04,0x0a,0x01,0x12,0x03,0x43,0x08,0x25,0x0a,0x0b, +0x0a,0x04,0x04,0x0a,0x02,0x00,0x12,0x03,0x44,0x02, +0x2d,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x04, +0x12,0x03,0x44,0x02,0x0a,0x0a,0x0c,0x0a,0x05,0x04, +0x0a,0x02,0x00,0x06,0x12,0x03,0x44,0x0b,0x20,0x0a, +0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x01,0x12,0x03, +0x44,0x21,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02, +0x00,0x03,0x12,0x03,0x44,0x2b,0x2c,0x0a,0x0a,0x0a, +0x02,0x04,0x0b,0x12,0x04,0x47,0x00,0x4a,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x0b,0x01,0x12,0x03,0x47,0x08, +0x24,0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02,0x00,0x12, +0x03,0x48,0x02,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x0b, +0x02,0x00,0x05,0x12,0x03,0x48,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x0b,0x02,0x00,0x01,0x12,0x03,0x48, +0x09,0x11,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00, +0x03,0x12,0x03,0x48,0x14,0x15,0x0a,0x0b,0x0a,0x04, +0x04,0x0b,0x02,0x01,0x12,0x03,0x49,0x02,0x1f,0x0a, +0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x06,0x12,0x03, +0x49,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02, +0x01,0x01,0x12,0x03,0x49,0x15,0x1a,0x0a,0x0c,0x0a, +0x05,0x04,0x0b,0x02,0x01,0x03,0x12,0x03,0x49,0x1d, +0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12,0x04,0x4c, +0x00,0x4f,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0c,0x01, +0x12,0x03,0x4c,0x08,0x21,0x0a,0x0b,0x0a,0x04,0x04, +0x0c,0x02,0x00,0x12,0x03,0x4d,0x02,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x0c,0x02,0x00,0x05,0x12,0x03,0x4d, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00, +0x01,0x12,0x03,0x4d,0x09,0x0e,0x0a,0x0c,0x0a,0x05, +0x04,0x0c,0x02,0x00,0x03,0x12,0x03,0x4d,0x11,0x12, +0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01,0x12,0x03, +0x4e,0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02, +0x01,0x06,0x12,0x03,0x4e,0x02,0x14,0x0a,0x0c,0x0a, +0x05,0x04,0x0c,0x02,0x01,0x01,0x12,0x03,0x4e,0x15, +0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x03, +0x12,0x03,0x4e,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04, +0x0d,0x12,0x04,0x51,0x00,0x54,0x01,0x0a,0x0a,0x0a, +0x03,0x04,0x0d,0x01,0x12,0x03,0x51,0x08,0x29,0x0a, +0x0b,0x0a,0x04,0x04,0x0d,0x02,0x00,0x12,0x03,0x52, +0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00, +0x05,0x12,0x03,0x52,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x0d,0x02,0x00,0x01,0x12,0x03,0x52,0x09,0x15, +0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x03,0x12, +0x03,0x52,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x0d, +0x02,0x01,0x12,0x03,0x53,0x02,0x22,0x0a,0x0c,0x0a, +0x05,0x04,0x0d,0x02,0x01,0x05,0x12,0x03,0x53,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x01, +0x12,0x03,0x53,0x09,0x1d,0x0a,0x0c,0x0a,0x05,0x04, +0x0d,0x02,0x01,0x03,0x12,0x03,0x53,0x20,0x21,0x62, +0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, }; static const char file_name[] = "kinematics.proto"; static const char wpi_proto_ProtobufChassisSpeeds_name[] = "wpi.proto.ProtobufChassisSpeeds"; @@ -323,6 +439,12 @@ pb_filedesc_t wpi_proto_ProtobufChassisSpeeds::file_descriptor(void) noexcept { PB_BIND(wpi_proto_ProtobufChassisSpeeds, wpi_proto_ProtobufChassisSpeeds, AUTO) +static const char wpi_proto_ProtobufChassisAccelerations_name[] = "wpi.proto.ProtobufChassisAccelerations"; +std::string_view wpi_proto_ProtobufChassisAccelerations::msg_name(void) noexcept { return wpi_proto_ProtobufChassisAccelerations_name; } +pb_filedesc_t wpi_proto_ProtobufChassisAccelerations::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } +PB_BIND(wpi_proto_ProtobufChassisAccelerations, wpi_proto_ProtobufChassisAccelerations, AUTO) + + static const char wpi_proto_ProtobufDifferentialDriveKinematics_name[] = "wpi.proto.ProtobufDifferentialDriveKinematics"; std::string_view wpi_proto_ProtobufDifferentialDriveKinematics::msg_name(void) noexcept { return wpi_proto_ProtobufDifferentialDriveKinematics_name; } pb_filedesc_t wpi_proto_ProtobufDifferentialDriveKinematics::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } @@ -335,6 +457,12 @@ pb_filedesc_t wpi_proto_ProtobufDifferentialDriveWheelSpeeds::file_descriptor(vo PB_BIND(wpi_proto_ProtobufDifferentialDriveWheelSpeeds, wpi_proto_ProtobufDifferentialDriveWheelSpeeds, AUTO) +static const char wpi_proto_ProtobufDifferentialDriveWheelAccelerations_name[] = "wpi.proto.ProtobufDifferentialDriveWheelAccelerations"; +std::string_view wpi_proto_ProtobufDifferentialDriveWheelAccelerations::msg_name(void) noexcept { return wpi_proto_ProtobufDifferentialDriveWheelAccelerations_name; } +pb_filedesc_t wpi_proto_ProtobufDifferentialDriveWheelAccelerations::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } +PB_BIND(wpi_proto_ProtobufDifferentialDriveWheelAccelerations, wpi_proto_ProtobufDifferentialDriveWheelAccelerations, AUTO) + + static const char wpi_proto_ProtobufDifferentialDriveWheelPositions_name[] = "wpi.proto.ProtobufDifferentialDriveWheelPositions"; std::string_view wpi_proto_ProtobufDifferentialDriveWheelPositions::msg_name(void) noexcept { return wpi_proto_ProtobufDifferentialDriveWheelPositions_name; } pb_filedesc_t wpi_proto_ProtobufDifferentialDriveWheelPositions::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } @@ -359,6 +487,12 @@ pb_filedesc_t wpi_proto_ProtobufMecanumDriveWheelSpeeds::file_descriptor(void) n PB_BIND(wpi_proto_ProtobufMecanumDriveWheelSpeeds, wpi_proto_ProtobufMecanumDriveWheelSpeeds, AUTO) +static const char wpi_proto_ProtobufMecanumDriveWheelAccelerations_name[] = "wpi.proto.ProtobufMecanumDriveWheelAccelerations"; +std::string_view wpi_proto_ProtobufMecanumDriveWheelAccelerations::msg_name(void) noexcept { return wpi_proto_ProtobufMecanumDriveWheelAccelerations_name; } +pb_filedesc_t wpi_proto_ProtobufMecanumDriveWheelAccelerations::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } +PB_BIND(wpi_proto_ProtobufMecanumDriveWheelAccelerations, wpi_proto_ProtobufMecanumDriveWheelAccelerations, AUTO) + + static const char wpi_proto_ProtobufSwerveDriveKinematics_name[] = "wpi.proto.ProtobufSwerveDriveKinematics"; std::string_view wpi_proto_ProtobufSwerveDriveKinematics::msg_name(void) noexcept { return wpi_proto_ProtobufSwerveDriveKinematics_name; } pb_filedesc_t wpi_proto_ProtobufSwerveDriveKinematics::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } @@ -377,6 +511,12 @@ pb_filedesc_t wpi_proto_ProtobufSwerveModuleState::file_descriptor(void) noexcep PB_BIND(wpi_proto_ProtobufSwerveModuleState, wpi_proto_ProtobufSwerveModuleState, AUTO) +static const char wpi_proto_ProtobufSwerveModuleAccelerations_name[] = "wpi.proto.ProtobufSwerveModuleAccelerations"; +std::string_view wpi_proto_ProtobufSwerveModuleAccelerations::msg_name(void) noexcept { return wpi_proto_ProtobufSwerveModuleAccelerations_name; } +pb_filedesc_t wpi_proto_ProtobufSwerveModuleAccelerations::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } +PB_BIND(wpi_proto_ProtobufSwerveModuleAccelerations, wpi_proto_ProtobufSwerveModuleAccelerations, AUTO) + + #ifndef PB_CONVERT_DOUBLE_FLOAT /* On some platforms (such as AVR), double is really float. diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h index 0f8900c8cbc..cd31c0c358c 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h @@ -26,6 +26,16 @@ typedef struct _wpi_proto_ProtobufChassisSpeeds { double omega; } wpi_proto_ProtobufChassisSpeeds; +typedef struct _wpi_proto_ProtobufChassisAccelerations { + static const pb_msgdesc_t* msg_descriptor(void) noexcept; + static std::string_view msg_name(void) noexcept; + static pb_filedesc_t file_descriptor(void) noexcept; + + double ax; + double ay; + double alpha; +} wpi_proto_ProtobufChassisAccelerations; + typedef struct _wpi_proto_ProtobufDifferentialDriveKinematics { static const pb_msgdesc_t* msg_descriptor(void) noexcept; static std::string_view msg_name(void) noexcept; @@ -43,6 +53,15 @@ typedef struct _wpi_proto_ProtobufDifferentialDriveWheelSpeeds { double right; } wpi_proto_ProtobufDifferentialDriveWheelSpeeds; +typedef struct _wpi_proto_ProtobufDifferentialDriveWheelAccelerations { + static const pb_msgdesc_t* msg_descriptor(void) noexcept; + static std::string_view msg_name(void) noexcept; + static pb_filedesc_t file_descriptor(void) noexcept; + + double left; + double right; +} wpi_proto_ProtobufDifferentialDriveWheelAccelerations; + typedef struct _wpi_proto_ProtobufDifferentialDriveWheelPositions { static const pb_msgdesc_t* msg_descriptor(void) noexcept; static std::string_view msg_name(void) noexcept; @@ -85,6 +104,17 @@ typedef struct _wpi_proto_ProtobufMecanumDriveWheelSpeeds { double rear_right; } wpi_proto_ProtobufMecanumDriveWheelSpeeds; +typedef struct _wpi_proto_ProtobufMecanumDriveWheelAccelerations { + static const pb_msgdesc_t* msg_descriptor(void) noexcept; + static std::string_view msg_name(void) noexcept; + static pb_filedesc_t file_descriptor(void) noexcept; + + double front_left; + double front_right; + double rear_left; + double rear_right; +} wpi_proto_ProtobufMecanumDriveWheelAccelerations; + typedef struct _wpi_proto_ProtobufSwerveDriveKinematics { static const pb_msgdesc_t* msg_descriptor(void) noexcept; static std::string_view msg_name(void) noexcept; @@ -111,36 +141,58 @@ typedef struct _wpi_proto_ProtobufSwerveModuleState { pb_callback_t angle; } wpi_proto_ProtobufSwerveModuleState; +typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { + static const pb_msgdesc_t* msg_descriptor(void) noexcept; + static std::string_view msg_name(void) noexcept; + static pb_filedesc_t file_descriptor(void) noexcept; + + double acceleration; + double angular_acceleration; +} wpi_proto_ProtobufSwerveModuleAccelerations; + /* Initializer values for message structs */ #define wpi_proto_ProtobufChassisSpeeds_init_default {0, 0, 0} +#define wpi_proto_ProtobufChassisAccelerations_init_default {0, 0, 0} #define wpi_proto_ProtobufDifferentialDriveKinematics_init_default {0} #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_init_default {0, 0} +#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_init_default {0, 0} #define wpi_proto_ProtobufDifferentialDriveWheelPositions_init_default {0, 0} #define wpi_proto_ProtobufMecanumDriveKinematics_init_default {{{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}} #define wpi_proto_ProtobufMecanumDriveWheelPositions_init_default {0, 0, 0, 0} #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_init_default {0, 0, 0, 0} +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_init_default {0, 0, 0, 0} #define wpi_proto_ProtobufSwerveDriveKinematics_init_default {{{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModulePosition_init_default {0, {{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModuleState_init_default {0, {{NULL}, NULL}} +#define wpi_proto_ProtobufSwerveModuleAccelerations_init_default {0, 0} #define wpi_proto_ProtobufChassisSpeeds_init_zero {0, 0, 0} +#define wpi_proto_ProtobufChassisAccelerations_init_zero {0, 0, 0} #define wpi_proto_ProtobufDifferentialDriveKinematics_init_zero {0} #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_init_zero {0, 0} +#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_init_zero {0, 0} #define wpi_proto_ProtobufDifferentialDriveWheelPositions_init_zero {0, 0} #define wpi_proto_ProtobufMecanumDriveKinematics_init_zero {{{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}} #define wpi_proto_ProtobufMecanumDriveWheelPositions_init_zero {0, 0, 0, 0} #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_init_zero {0, 0, 0, 0} +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_init_zero {0, 0, 0, 0} #define wpi_proto_ProtobufSwerveDriveKinematics_init_zero {{{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModulePosition_init_zero {0, {{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModuleState_init_zero {0, {{NULL}, NULL}} +#define wpi_proto_ProtobufSwerveModuleAccelerations_init_zero {0, 0} /* Field tags (for use in manual encoding/decoding) */ #define wpi_proto_ProtobufChassisSpeeds_vx_tag 1 #define wpi_proto_ProtobufChassisSpeeds_vy_tag 2 #define wpi_proto_ProtobufChassisSpeeds_omega_tag 3 +#define wpi_proto_ProtobufChassisAccelerations_ax_tag 1 +#define wpi_proto_ProtobufChassisAccelerations_ay_tag 2 +#define wpi_proto_ProtobufChassisAccelerations_alpha_tag 3 #define wpi_proto_ProtobufDifferentialDriveKinematics_trackwidth_tag 1 #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_left_tag 1 #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_right_tag 2 +#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_left_tag 1 +#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_right_tag 2 #define wpi_proto_ProtobufDifferentialDriveWheelPositions_left_tag 1 #define wpi_proto_ProtobufDifferentialDriveWheelPositions_right_tag 2 #define wpi_proto_ProtobufMecanumDriveKinematics_front_left_tag 1 @@ -155,11 +207,17 @@ typedef struct _wpi_proto_ProtobufSwerveModuleState { #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_front_right_tag 2 #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_rear_left_tag 3 #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_rear_right_tag 4 +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_front_left_tag 1 +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_front_right_tag 2 +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_rear_left_tag 3 +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_rear_right_tag 4 #define wpi_proto_ProtobufSwerveDriveKinematics_modules_tag 1 #define wpi_proto_ProtobufSwerveModulePosition_distance_tag 1 #define wpi_proto_ProtobufSwerveModulePosition_angle_tag 2 #define wpi_proto_ProtobufSwerveModuleState_speed_tag 1 #define wpi_proto_ProtobufSwerveModuleState_angle_tag 2 +#define wpi_proto_ProtobufSwerveModuleAccelerations_acceleration_tag 1 +#define wpi_proto_ProtobufSwerveModuleAccelerations_angular_acceleration_tag 2 /* Struct field encoding specification for nanopb */ #define wpi_proto_ProtobufChassisSpeeds_FIELDLIST(X, a) \ @@ -169,6 +227,13 @@ X(a, STATIC, SINGULAR, DOUBLE, omega, 3) #define wpi_proto_ProtobufChassisSpeeds_CALLBACK NULL #define wpi_proto_ProtobufChassisSpeeds_DEFAULT NULL +#define wpi_proto_ProtobufChassisAccelerations_FIELDLIST(X, a) \ +X(a, STATIC, SINGULAR, DOUBLE, ax, 1) \ +X(a, STATIC, SINGULAR, DOUBLE, ay, 2) \ +X(a, STATIC, SINGULAR, DOUBLE, alpha, 3) +#define wpi_proto_ProtobufChassisAccelerations_CALLBACK NULL +#define wpi_proto_ProtobufChassisAccelerations_DEFAULT NULL + #define wpi_proto_ProtobufDifferentialDriveKinematics_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, DOUBLE, trackwidth, 1) #define wpi_proto_ProtobufDifferentialDriveKinematics_CALLBACK NULL @@ -180,6 +245,12 @@ X(a, STATIC, SINGULAR, DOUBLE, right, 2) #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_CALLBACK NULL #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_DEFAULT NULL +#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_FIELDLIST(X, a) \ +X(a, STATIC, SINGULAR, DOUBLE, left, 1) \ +X(a, STATIC, SINGULAR, DOUBLE, right, 2) +#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_CALLBACK NULL +#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_DEFAULT NULL + #define wpi_proto_ProtobufDifferentialDriveWheelPositions_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, DOUBLE, left, 1) \ X(a, STATIC, SINGULAR, DOUBLE, right, 2) @@ -214,6 +285,14 @@ X(a, STATIC, SINGULAR, DOUBLE, rear_right, 4) #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_CALLBACK NULL #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_DEFAULT NULL +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_FIELDLIST(X, a) \ +X(a, STATIC, SINGULAR, DOUBLE, front_left, 1) \ +X(a, STATIC, SINGULAR, DOUBLE, front_right, 2) \ +X(a, STATIC, SINGULAR, DOUBLE, rear_left, 3) \ +X(a, STATIC, SINGULAR, DOUBLE, rear_right, 4) +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_CALLBACK NULL +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_DEFAULT NULL + #define wpi_proto_ProtobufSwerveDriveKinematics_FIELDLIST(X, a) \ X(a, CALLBACK, REPEATED, MESSAGE, modules, 1) #define wpi_proto_ProtobufSwerveDriveKinematics_CALLBACK pb_default_field_callback @@ -234,18 +313,28 @@ X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2) #define wpi_proto_ProtobufSwerveModuleState_DEFAULT NULL #define wpi_proto_ProtobufSwerveModuleState_angle_MSGTYPE wpi_proto_ProtobufRotation2d +#define wpi_proto_ProtobufSwerveModuleAccelerations_FIELDLIST(X, a) \ +X(a, STATIC, SINGULAR, DOUBLE, acceleration, 1) \ +X(a, STATIC, SINGULAR, DOUBLE, angular_acceleration, 2) +#define wpi_proto_ProtobufSwerveModuleAccelerations_CALLBACK NULL +#define wpi_proto_ProtobufSwerveModuleAccelerations_DEFAULT NULL + /* Maximum encoded size of messages (where known) */ /* wpi_proto_ProtobufMecanumDriveKinematics_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveDriveKinematics_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveModulePosition_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveModuleState_size depends on runtime parameters */ #define WPI_PROTO_KINEMATICS_NPB_H_MAX_SIZE wpi_proto_ProtobufMecanumDriveWheelPositions_size +#define wpi_proto_ProtobufChassisAccelerations_size 27 #define wpi_proto_ProtobufChassisSpeeds_size 27 #define wpi_proto_ProtobufDifferentialDriveKinematics_size 9 +#define wpi_proto_ProtobufDifferentialDriveWheelAccelerations_size 18 #define wpi_proto_ProtobufDifferentialDriveWheelPositions_size 18 #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_size 18 +#define wpi_proto_ProtobufMecanumDriveWheelAccelerations_size 36 #define wpi_proto_ProtobufMecanumDriveWheelPositions_size 36 #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_size 36 +#define wpi_proto_ProtobufSwerveModuleAccelerations_size 18 #endif diff --git a/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp new file mode 100644 index 00000000000..d2f9cc1ba57 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp @@ -0,0 +1,31 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/proto/ChassisAccelerationsProto.h" + +#include "wpimath/protobuf/kinematics.npb.h" + +std::optional wpi::Protobuf::Unpack( + InputStream& stream) { + wpi_proto_ProtobufChassisAccelerations msg; + if (!stream.Decode(msg)) { + return {}; + } + + return frc::ChassisAccelerations{ + units::meters_per_second_squared_t{msg.ax}, + units::meters_per_second_squared_t{msg.ay}, + units::radians_per_second_squared_t{msg.alpha}, + }; +} + +bool wpi::Protobuf::Pack(OutputStream& stream, + const frc::ChassisAccelerations& value) { + wpi_proto_ProtobufChassisAccelerations msg{ + .ax = value.ax.value(), + .ay = value.ay.value(), + .alpha = value.alpha.value(), + }; + return stream.Encode(msg); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp new file mode 100644 index 00000000000..a522d910ee6 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp @@ -0,0 +1,34 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/struct/ChassisAccelerationsStruct.h" + +#include + +#include "frc/kinematics/ChassisAccelerations.h" + +frc::ChassisAccelerations wpi::Struct::Unpack( + std::span data) { + constexpr size_t kAxOff = 0; + constexpr size_t kAyOff = kAxOff + 8; + constexpr size_t kAlphaOff = kAyOff + 8; + return frc::ChassisAccelerations{ + units::meters_per_second_squared_t{ + wpi::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::UnpackStruct(data)}, + units::radians_per_second_squared_t{ + wpi::UnpackStruct(data)}, + }; +} + +void wpi::Struct::Pack( + std::span data, const frc::ChassisAccelerations& value) { + constexpr size_t kAxOff = 0; + constexpr size_t kAyOff = kAxOff + 8; + constexpr size_t kAlphaOff = kAyOff + 8; + wpi::PackStruct(data, value.ax.value()); + wpi::PackStruct(data, value.ay.value()); + wpi::PackStruct(data, value.alpha.value()); +} diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h new file mode 100644 index 00000000000..1bceaeebc8d --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h @@ -0,0 +1,161 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Translation2d.h" +#include "units/acceleration.h" +#include "units/angular_acceleration.h" + +namespace frc { +/** + * Represents the acceleration of a robot chassis. Although this struct contains + * similar members compared to a Twist2d, they do NOT represent the same thing. + * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of + * reference, a ChassisAccelerations struct represents a robot's acceleration. + * + * A strictly non-holonomic drivetrain, such as a differential drive, should + * never have an ay component because it can never accelerate sideways. + * Holonomic drivetrains such as swerve and mecanum will often have all three + * components. + */ +struct WPILIB_DLLEXPORT ChassisAccelerations { + /** + * Acceleration along the x-axis. (Fwd is +) + */ + units::meters_per_second_squared_t ax = 0_mps_sq; + + /** + * Acceleration along the y-axis. (Left is +) + */ + units::meters_per_second_squared_t ay = 0_mps_sq; + + /** + * Represents the angular acceleration of the robot frame. (CCW is +) + */ + units::radians_per_second_squared_t alpha = 0_rad_per_s_sq; + + /** + * Converts this field-relative set of accelerations into a robot-relative + * ChassisAccelerations object. + * + * @param robotAngle The angle of the robot as measured by a gyroscope. The + * robot's angle is considered to be zero when it is facing directly away + * from your alliance station wall. Remember that this should be CCW + * positive. + * @return ChassisAccelerations object representing the accelerations in the + * robot's frame of reference. + */ + constexpr ChassisAccelerations ToRobotRelative( + const Rotation2d& robotAngle) const { + // CW rotation into chassis frame + auto rotated = + Translation2d{units::meter_t{ax.value()}, units::meter_t{ay.value()}} + .RotateBy(-robotAngle); + return {units::meters_per_second_squared_t{rotated.X().value()}, + units::meters_per_second_squared_t{rotated.Y().value()}, alpha}; + } + + /** + * Converts this robot-relative set of accelerations into a field-relative + * ChassisAccelerations object. + * + * @param robotAngle The angle of the robot as measured by a gyroscope. The + * robot's angle is considered to be zero when it is facing directly away + * from your alliance station wall. Remember that this should be CCW + * positive. + * @return ChassisAccelerations object representing the accelerations in the + * field's frame of reference. + */ + constexpr ChassisAccelerations ToFieldRelative( + const Rotation2d& robotAngle) const { + // CCW rotation out of chassis frame + auto rotated = + Translation2d{units::meter_t{ax.value()}, units::meter_t{ay.value()}} + .RotateBy(robotAngle); + return {units::meters_per_second_squared_t{rotated.X().value()}, + units::meters_per_second_squared_t{rotated.Y().value()}, alpha}; + } + + /** + * Adds two ChassisAccelerations and returns the sum. + * + *

For example, ChassisAccelerations{1.0, 0.5, 1.5} + ChassisAccelerations{2.0, 1.5, 0.5} + * = ChassisAccelerations{3.0, 2.0, 2.0} + * + * @param other The ChassisAccelerations to add. + * + * @return The sum of the ChassisAccelerations. + */ + constexpr ChassisAccelerations operator+(const ChassisAccelerations& other) const { + return {ax + other.ax, ay + other.ay, alpha + other.alpha}; + } + + /** + * Subtracts the other ChassisAccelerations from the current ChassisAccelerations and + * returns the difference. + * + *

For example, ChassisAccelerations{5.0, 4.0, 2.0} - ChassisAccelerations{1.0, 2.0, 1.0} + * = ChassisAccelerations{4.0, 2.0, 1.0} + * + * @param other The ChassisAccelerations to subtract. + * + * @return The difference between the two ChassisAccelerations. + */ + constexpr ChassisAccelerations operator-(const ChassisAccelerations& other) const { + return *this + -other; + } + + /** + * Returns the inverse of the current ChassisAccelerations. + * This is equivalent to negating all components of the ChassisAccelerations. + * + * @return The inverse of the current ChassisAccelerations. + */ + constexpr ChassisAccelerations operator-() const { return {-ax, -ay, -alpha}; } + + /** + * Multiplies the ChassisAccelerations by a scalar and returns the new ChassisAccelerations. + * + *

For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2 + * = ChassisAccelerations{4.0, 5.0, 1.0} + * + * @param scalar The scalar to multiply by. + * + * @return The scaled ChassisAccelerations. + */ + constexpr ChassisAccelerations operator*(double scalar) const { + return {scalar * ax, scalar * ay, scalar * alpha}; + } + + /** + * Divides the ChassisAccelerations by a scalar and returns the new ChassisAccelerations. + * + *

For example, ChassisAccelerations{2.0, 2.5, 1.0} / 2 + * = ChassisAccelerations{1.0, 1.25, 0.5} + * + * @param scalar The scalar to divide by. + * + * @return The scaled ChassisAccelerations. + */ + constexpr ChassisAccelerations operator/(double scalar) const { + return operator*(1.0 / scalar); + } + + /** + * Compares the ChassisAccelerations with another ChassisAccelerations. + * + * @param other The other ChassisAccelerations. + * + * @return The result of the comparison. Is true if they are the same. + */ + constexpr bool operator==(const ChassisAccelerations& other) const = default; +}; +} // namespace frc + +#include "frc/kinematics/proto/ChassisAccelerationsProto.h" +#include "frc/kinematics/struct/ChassisAccelerationsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/ChassisAccelerationsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/ChassisAccelerationsProto.h new file mode 100644 index 00000000000..d4fc920fc14 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/ChassisAccelerationsProto.h @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/ChassisAccelerations.h" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + using MessageStruct = wpi_proto_ProtobufChassisAccelerations; + using InputStream = wpi::ProtoInputStream; + using OutputStream = wpi::ProtoOutputStream; + static std::optional Unpack(InputStream& stream); + static bool Pack(OutputStream& stream, const frc::ChassisAccelerations& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/ChassisAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisAccelerationsStruct.h new file mode 100644 index 00000000000..34b45bf0fa6 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisAccelerationsStruct.h @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/ChassisAccelerations.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeName() { return "ChassisAccelerations"; } + static constexpr size_t GetSize() { return 24; } + static constexpr std::string_view GetSchema() { + return "double ax;double ay;double alpha"; + } + + static frc::ChassisAccelerations Unpack(std::span data); + static void Pack(std::span data, const frc::ChassisAccelerations& value); +}; + +static_assert(wpi::StructSerializable); From 5c6329c3cac8f65ae7e15d57863613234da3a803 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 17:01:39 -0400 Subject: [PATCH 07/82] feat: create DifferentialDriveWheelAccelerations, MecanumDriveWheelAccelerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel --- .../edu/wpi/first/math/proto/Kinematics.java | 2000 +++++++++++++---- .../DifferentialDriveWheelAccelerations.java | 125 ++ .../MecanumDriveWheelAccelerations.java | 154 ++ .../kinematics/SwerveModuleAccelerations.java | 145 ++ ...ferentialDriveWheelAccelerationsProto.java | 39 + .../MecanumDriveWheelAccelerationsProto.java | 42 + .../proto/SwerveModuleAccelerationsProto.java | 39 + ...erentialDriveWheelAccelerationsStruct.java | 44 + .../MecanumDriveWheelAccelerationsStruct.java | 48 + .../SwerveModuleAccelerationsStruct.java | 44 + wpimath/src/main/proto/kinematics.proto | 17 + 11 files changed, 2296 insertions(+), 401 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java index 264e10937c8..a272a1380b6 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java @@ -19,62 +19,77 @@ import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3295, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4187, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + "BW9tZWdhIlQKHFByb3RvYnVmQ2hhc3Npc0FjY2VsZXJhdGlvbnMSDgoCYXgYASABKAFSAmF4Eg4KAmF5" + "GAIgASgBUgJheRIUCgVhbHBoYRgDIAEoAVIFYWxwaGEiRQojUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2" + "ZUtpbmVtYXRpY3MSHgoKdHJhY2t3aWR0aBgBIAEoAVIKdHJhY2t3aWR0aCJQCiRQcm90b2J1ZkRpZmZl" + "cmVudGlhbERyaXZlV2hlZWxTcGVlZHMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIF" + - "cmlnaHQiUwonUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsUG9zaXRpb25zEhIKBGxlZnQYASAB" + - "KAFSBGxlZnQSFAoFcmlnaHQYAiABKAFSBXJpZ2h0IqQCCh5Qcm90b2J1Zk1lY2FudW1Ecml2ZUtpbmVt" + - "YXRpY3MSPwoKZnJvbnRfbGVmdBgBIAEoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRS" + - "CWZyb250TGVmdBJBCgtmcm9udF9yaWdodBgCIAEoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0" + - "aW9uMmRSCmZyb250UmlnaHQSPQoJcmVhcl9sZWZ0GAMgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJh" + - "bnNsYXRpb24yZFIIcmVhckxlZnQSPwoKcmVhcl9yaWdodBgEIAEoCzIgLndwaS5wcm90by5Qcm90b2J1" + - "ZlRyYW5zbGF0aW9uMmRSCXJlYXJSaWdodCKgAQoiUHJvdG9idWZNZWNhbnVtRHJpdmVXaGVlbFBvc2l0" + - "aW9ucxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQYAiABKAFSCmZy" + - "b250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0GAQgASgBUgly" + - "ZWFyUmlnaHQinQEKH1Byb3RvYnVmTWVjYW51bURyaXZlV2hlZWxTcGVlZHMSHQoKZnJvbnRfbGVmdBgB" + - "IAEoAVIJZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVm" + - "dBgDIAEoAVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0IlsKHVByb3RvYnVm" + - "U3dlcnZlRHJpdmVLaW5lbWF0aWNzEjoKB21vZHVsZXMYASADKAsyIC53cGkucHJvdG8uUHJvdG9idWZU" + - "cmFuc2xhdGlvbjJkUgdtb2R1bGVzIm8KHFByb3RvYnVmU3dlcnZlTW9kdWxlUG9zaXRpb24SGgoIZGlz", - "dGFuY2UYASABKAFSCGRpc3RhbmNlEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90" + - "YXRpb24yZFIFYW5nbGUiZgoZUHJvdG9idWZTd2VydmVNb2R1bGVTdGF0ZRIUCgVzcGVlZBgBIAEoAVIF" + - "c3BlZWQSMwoFYW5nbGUYAiABKAsyHS53cGkucHJvdG8uUHJvdG9idWZSb3RhdGlvbjJkUgVhbmdsZUIa" + - "ChhlZHUud3BpLmZpcnN0Lm1hdGgucHJvdG9K1g4KBhIEAABDAQoICgEMEgMAABIKCAoBAhIDAgASCgkK" + - "AgMAEgMEABoKCAoBCBIDBgAxCgkKAggBEgMGADEKCgoCBAASBAgADAEKCgoDBAABEgMICB0KCwoEBAAC" + - "ABIDCQIQCgwKBQQAAgAFEgMJAggKDAoFBAACAAESAwkJCwoMCgUEAAIAAxIDCQ4PCgsKBAQAAgESAwoC" + - "EAoMCgUEAAIBBRIDCgIICgwKBQQAAgEBEgMKCQsKDAoFBAACAQMSAwoODwoLCgQEAAICEgMLAhMKDAoF" + - "BAACAgUSAwsCCAoMCgUEAAICARIDCwkOCgwKBQQAAgIDEgMLERIKCgoCBAESBA4AEgEKCgoDBAEBEgMO" + - "CCQKCwoEBAECABIDDwIQCgwKBQQBAgAFEgMPAggKDAoFBAECAAESAw8JCwoMCgUEAQIAAxIDDw4PCgsK" + - "BAQBAgESAxACEAoMCgUEAQIBBRIDEAIICgwKBQQBAgEBEgMQCQsKDAoFBAECAQMSAxAODwoLCgQEAQIC" + - "EgMRAhMKDAoFBAECAgUSAxECCAoMCgUEAQICARIDEQkOCgwKBQQBAgIDEgMRERIKCgoCBAISBBQAFgEK" + - "CgoDBAIBEgMUCCsKCwoEBAICABIDFQIYCgwKBQQCAgAFEgMVAggKDAoFBAICAAESAxUJEwoMCgUEAgIA" + - "AxIDFRYXCgoKAgQDEgQYABsBCgoKAwQDARIDGAgsCgsKBAQDAgASAxkCEgoMCgUEAwIABRIDGQIICgwK" + - "BQQDAgABEgMZCQ0KDAoFBAMCAAMSAxkQEQoLCgQEAwIBEgMaAhMKDAoFBAMCAQUSAxoCCAoMCgUEAwIB" + - "ARIDGgkOCgwKBQQDAgEDEgMaERIKCgoCBAQSBB0AIAEKCgoDBAQBEgMdCC8KCwoEBAQCABIDHgISCgwK" + - "BQQEAgAFEgMeAggKDAoFBAQCAAESAx4JDQoMCgUEBAIAAxIDHhARCgsKBAQEAgESAx8CEwoMCgUEBAIB" + - "BRIDHwIICgwKBQQEAgEBEgMfCQ4KDAoFBAQCAQMSAx8REgoKCgIEBRIEIgAnAQoKCgMEBQESAyIIJgoL" + - "CgQEBQIAEgMjAicKDAoFBAUCAAYSAyMCFwoMCgUEBQIAARIDIxgiCgwKBQQFAgADEgMjJSYKCwoEBAUC" + - "ARIDJAIoCgwKBQQFAgEGEgMkAhcKDAoFBAUCAQESAyQYIwoMCgUEBQIBAxIDJCYnCgsKBAQFAgISAyUC" + - "JgoMCgUEBQICBhIDJQIXCgwKBQQFAgIBEgMlGCEKDAoFBAUCAgMSAyUkJQoLCgQEBQIDEgMmAicKDAoF", - "BAUCAwYSAyYCFwoMCgUEBQIDARIDJhgiCgwKBQQFAgMDEgMmJSYKCgoCBAYSBCkALgEKCgoDBAYBEgMp" + - "CCoKCwoEBAYCABIDKgIYCgwKBQQGAgAFEgMqAggKDAoFBAYCAAESAyoJEwoMCgUEBgIAAxIDKhYXCgsK" + - "BAQGAgESAysCGQoMCgUEBgIBBRIDKwIICgwKBQQGAgEBEgMrCRQKDAoFBAYCAQMSAysXGAoLCgQEBgIC" + - "EgMsAhcKDAoFBAYCAgUSAywCCAoMCgUEBgICARIDLAkSCgwKBQQGAgIDEgMsFRYKCwoEBAYCAxIDLQIY" + - "CgwKBQQGAgMFEgMtAggKDAoFBAYCAwESAy0JEwoMCgUEBgIDAxIDLRYXCgoKAgQHEgQwADUBCgoKAwQH" + - "ARIDMAgnCgsKBAQHAgASAzECGAoMCgUEBwIABRIDMQIICgwKBQQHAgABEgMxCRMKDAoFBAcCAAMSAzEW" + - "FwoLCgQEBwIBEgMyAhkKDAoFBAcCAQUSAzICCAoMCgUEBwIBARIDMgkUCgwKBQQHAgEDEgMyFxgKCwoE" + - "BAcCAhIDMwIXCgwKBQQHAgIFEgMzAggKDAoFBAcCAgESAzMJEgoMCgUEBwICAxIDMxUWCgsKBAQHAgMS" + - "AzQCGAoMCgUEBwIDBRIDNAIICgwKBQQHAgMBEgM0CRMKDAoFBAcCAwMSAzQWFwoKCgIECBIENwA5AQoK" + - "CgMECAESAzcIJQoLCgQECAIAEgM4Ai0KDAoFBAgCAAQSAzgCCgoMCgUECAIABhIDOAsgCgwKBQQIAgAB" + - "EgM4ISgKDAoFBAgCAAMSAzgrLAoKCgIECRIEOwA+AQoKCgMECQESAzsIJAoLCgQECQIAEgM8AhYKDAoF" + - "BAkCAAUSAzwCCAoMCgUECQIAARIDPAkRCgwKBQQJAgADEgM8FBUKCwoEBAkCARIDPQIfCgwKBQQJAgEG" + - "EgM9AhQKDAoFBAkCAQESAz0VGgoMCgUECQIBAxIDPR0eCgoKAgQKEgRAAEMBCgoKAwQKARIDQAghCgsK" + - "BAQKAgASA0ECEwoMCgUECgIABRIDQQIICgwKBQQKAgABEgNBCQ4KDAoFBAoCAAMSA0EREgoLCgQECgIB" + - "EgNCAh8KDAoFBAoCAQYSA0ICFAoMCgUECgIBARIDQhUaCgwKBQQKAgEDEgNCHR5iBnByb3RvMw=="); + "cmlnaHQiVworUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsQWNjZWxlcmF0aW9ucxISCgRsZWZ0" + + "GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCJTCidQcm90b2J1ZkRpZmZlcmVudGlhbERy" + + "aXZlV2hlZWxQb3NpdGlvbnMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIFcmlnaHQi" + + "pAIKHlByb3RvYnVmTWVjYW51bURyaXZlS2luZW1hdGljcxI/Cgpmcm9udF9sZWZ0GAEgASgLMiAud3Bp" + + "LnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJZnJvbnRMZWZ0EkEKC2Zyb250X3JpZ2h0GAIgASgL" + + "MiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIKZnJvbnRSaWdodBI9CglyZWFyX2xlZnQY" + + "AyABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUghyZWFyTGVmdBI/CgpyZWFyX3Jp" + + "Z2h0GAQgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJcmVhclJpZ2h0IqABCiJQ" + + "cm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250" + + "TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJl" + + "YXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNhbnVtRHJp" + + "dmVXaGVlbFNwZWVkcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQY" + + "AiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0" + + "GAQgASgBUglyZWFyUmlnaHQipAEKJlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxBY2NlbGVyYXRpb25z", + "Eh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRS" + + "aWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJS" + + "aWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURyaXZlS2luZW1hdGljcxI6Cgdtb2R1bGVzGAEgAygLMiAud3Bp" + + "LnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIHbW9kdWxlcyJvChxQcm90b2J1ZlN3ZXJ2ZU1vZHVs" + + "ZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5w" + + "cm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUS" + + "FAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90" + + "YXRpb24yZFIFYW5nbGUiegohUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb25zEiIKDGFjY2Vs" + + "ZXJhdGlvbhgBIAEoAVIMYWNjZWxlcmF0aW9uEjEKFGFuZ3VsYXJfYWNjZWxlcmF0aW9uGAIgASgBUhNh" + + "bmd1bGFyQWNjZWxlcmF0aW9uQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0rWEgoGEgQAAFQBCggK" + + "AQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGADEKCQoCCAESAwYAMQoKCgIEABIECAAM" + + "AQoKCgMEAAESAwgIHQoLCgQEAAIAEgMJAhAKDAoFBAACAAUSAwkCCAoMCgUEAAIAARIDCQkLCgwKBQQA" + + "AgADEgMJDg8KCwoEBAACARIDCgIQCgwKBQQAAgEFEgMKAggKDAoFBAACAQESAwoJCwoMCgUEAAIBAxID" + + "Cg4PCgsKBAQAAgISAwsCEwoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCQ4KDAoFBAACAgMSAwsREgoK" + + "CgIEARIEDgASAQoKCgMEAQESAw4IJAoLCgQEAQIAEgMPAhAKDAoFBAECAAUSAw8CCAoMCgUEAQIAARID" + + "DwkLCgwKBQQBAgADEgMPDg8KCwoEBAECARIDEAIQCgwKBQQBAgEFEgMQAggKDAoFBAECAQESAxAJCwoM" + + "CgUEAQIBAxIDEA4PCgsKBAQBAgISAxECEwoMCgUEAQICBRIDEQIICgwKBQQBAgIBEgMRCQ4KDAoFBAEC" + + "AgMSAxEREgoKCgIEAhIEFAAWAQoKCgMEAgESAxQIKwoLCgQEAgIAEgMVAhgKDAoFBAICAAUSAxUCCAoM" + + "CgUEAgIAARIDFQkTCgwKBQQCAgADEgMVFhcKCgoCBAMSBBgAGwEKCgoDBAMBEgMYCCwKCwoEBAMCABID" + + "GQISCgwKBQQDAgAFEgMZAggKDAoFBAMCAAESAxkJDQoMCgUEAwIAAxIDGRARCgsKBAQDAgESAxoCEwoM", + "CgUEAwIBBRIDGgIICgwKBQQDAgEBEgMaCQ4KDAoFBAMCAQMSAxoREgoKCgIEBBIEHQAgAQoKCgMEBAES" + + "Ax0IMwoLCgQEBAIAEgMeAhIKDAoFBAQCAAUSAx4CCAoMCgUEBAIAARIDHgkNCgwKBQQEAgADEgMeEBEK" + + "CwoEBAQCARIDHwITCgwKBQQEAgEFEgMfAggKDAoFBAQCAQESAx8JDgoMCgUEBAIBAxIDHxESCgoKAgQF" + + "EgQiACUBCgoKAwQFARIDIggvCgsKBAQFAgASAyMCEgoMCgUEBQIABRIDIwIICgwKBQQFAgABEgMjCQ0K" + + "DAoFBAUCAAMSAyMQEQoLCgQEBQIBEgMkAhMKDAoFBAUCAQUSAyQCCAoMCgUEBQIBARIDJAkOCgwKBQQF" + + "AgEDEgMkERIKCgoCBAYSBCcALAEKCgoDBAYBEgMnCCYKCwoEBAYCABIDKAInCgwKBQQGAgAGEgMoAhcK" + + "DAoFBAYCAAESAygYIgoMCgUEBgIAAxIDKCUmCgsKBAQGAgESAykCKAoMCgUEBgIBBhIDKQIXCgwKBQQG" + + "AgEBEgMpGCMKDAoFBAYCAQMSAykmJwoLCgQEBgICEgMqAiYKDAoFBAYCAgYSAyoCFwoMCgUEBgICARID" + + "KhghCgwKBQQGAgIDEgMqJCUKCwoEBAYCAxIDKwInCgwKBQQGAgMGEgMrAhcKDAoFBAYCAwESAysYIgoM" + + "CgUEBgIDAxIDKyUmCgoKAgQHEgQuADMBCgoKAwQHARIDLggqCgsKBAQHAgASAy8CGAoMCgUEBwIABRID" + + "LwIICgwKBQQHAgABEgMvCRMKDAoFBAcCAAMSAy8WFwoLCgQEBwIBEgMwAhkKDAoFBAcCAQUSAzACCAoM" + + "CgUEBwIBARIDMAkUCgwKBQQHAgEDEgMwFxgKCwoEBAcCAhIDMQIXCgwKBQQHAgIFEgMxAggKDAoFBAcC" + + "AgESAzEJEgoMCgUEBwICAxIDMRUWCgsKBAQHAgMSAzICGAoMCgUEBwIDBRIDMgIICgwKBQQHAgMBEgMy" + + "CRMKDAoFBAcCAwMSAzIWFwoKCgIECBIENQA6AQoKCgMECAESAzUIJwoLCgQECAIAEgM2AhgKDAoFBAgC" + + "AAUSAzYCCAoMCgUECAIAARIDNgkTCgwKBQQIAgADEgM2FhcKCwoEBAgCARIDNwIZCgwKBQQIAgEFEgM3" + + "AggKDAoFBAgCAQESAzcJFAoMCgUECAIBAxIDNxcYCgsKBAQIAgISAzgCFwoMCgUECAICBRIDOAIICgwK" + + "BQQIAgIBEgM4CRIKDAoFBAgCAgMSAzgVFgoLCgQECAIDEgM5AhgKDAoFBAgCAwUSAzkCCAoMCgUECAID" + + "ARIDOQkTCgwKBQQIAgMDEgM5FhcKCgoCBAkSBDwAQQEKCgoDBAkBEgM8CC4KCwoEBAkCABIDPQIYCgwK" + + "BQQJAgAFEgM9AggKDAoFBAkCAAESAz0JEwoMCgUECQIAAxIDPRYXCgsKBAQJAgESAz4CGQoMCgUECQIB" + + "BRIDPgIICgwKBQQJAgEBEgM+CRQKDAoFBAkCAQMSAz4XGAoLCgQECQICEgM/AhcKDAoFBAkCAgUSAz8C", + "CAoMCgUECQICARIDPwkSCgwKBQQJAgIDEgM/FRYKCwoEBAkCAxIDQAIYCgwKBQQJAgMFEgNAAggKDAoF" + + "BAkCAwESA0AJEwoMCgUECQIDAxIDQBYXCgoKAgQKEgRDAEUBCgoKAwQKARIDQwglCgsKBAQKAgASA0QC" + + "LQoMCgUECgIABBIDRAIKCgwKBQQKAgAGEgNECyAKDAoFBAoCAAESA0QhKAoMCgUECgIAAxIDRCssCgoK" + + "AgQLEgRHAEoBCgoKAwQLARIDRwgkCgsKBAQLAgASA0gCFgoMCgUECwIABRIDSAIICgwKBQQLAgABEgNI" + + "CREKDAoFBAsCAAMSA0gUFQoLCgQECwIBEgNJAh8KDAoFBAsCAQYSA0kCFAoMCgUECwIBARIDSRUaCgwK" + + "BQQLAgEDEgNJHR4KCgoCBAwSBEwATwEKCgoDBAwBEgNMCCEKCwoEBAwCABIDTQITCgwKBQQMAgAFEgNN" + + "AggKDAoFBAwCAAESA00JDgoMCgUEDAIAAxIDTRESCgsKBAQMAgESA04CHwoMCgUEDAIBBhIDTgIUCgwK" + + "BQQMAgEBEgNOFRoKDAoFBAwCAQMSA04dHgoKCgIEDRIEUQBUAQoKCgMEDQESA1EIKQoLCgQEDQIAEgNS" + + "AhoKDAoFBA0CAAUSA1ICCAoMCgUEDQIAARIDUgkVCgwKBQQNAgADEgNSGBkKCwoEBA0CARIDUwIiCgwK" + + "BQQNAgEFEgNTAggKDAoFBA0CAQESA1MJHQoMCgUEDQIBAxIDUyAhYgZwcm90bzM="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); @@ -86,19 +101,25 @@ public final class Kinematics { static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(285, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(367, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelAccelerations_descriptor = descriptor.internalContainedType(367, 87, "ProtobufDifferentialDriveWheelAccelerations", "wpi.proto.ProtobufDifferentialDriveWheelAccelerations"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(453, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(456, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(748, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(542, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(911, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(837, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1070, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(1000, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1163, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelAccelerations_descriptor = descriptor.internalContainedType(1160, 164, "ProtobufMecanumDriveWheelAccelerations", "wpi.proto.ProtobufMecanumDriveWheelAccelerations"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1276, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1326, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); + + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1419, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1532, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); + + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleAccelerations_descriptor = descriptor.internalContainedType(1636, 122, "ProtobufSwerveModuleAccelerations", "wpi.proto.ProtobufSwerveModuleAccelerations"); /** * @return this proto file's descriptor. @@ -1530,9 +1551,9 @@ static class FieldNames { } /** - * Protobuf type {@code ProtobufDifferentialDriveWheelPositions} + * Protobuf type {@code ProtobufDifferentialDriveWheelAccelerations} */ - public static final class ProtobufDifferentialDriveWheelPositions extends ProtoMessage implements Cloneable { + public static final class ProtobufDifferentialDriveWheelAccelerations extends ProtoMessage implements Cloneable { private static final long serialVersionUID = 0L; /** @@ -1545,14 +1566,14 @@ public static final class ProtobufDifferentialDriveWheelPositions extends ProtoM */ private double right; - private ProtobufDifferentialDriveWheelPositions() { + private ProtobufDifferentialDriveWheelAccelerations() { } /** - * @return a new empty instance of {@code ProtobufDifferentialDriveWheelPositions} + * @return a new empty instance of {@code ProtobufDifferentialDriveWheelAccelerations} */ - public static ProtobufDifferentialDriveWheelPositions newInstance() { - return new ProtobufDifferentialDriveWheelPositions(); + public static ProtobufDifferentialDriveWheelAccelerations newInstance() { + return new ProtobufDifferentialDriveWheelAccelerations(); } /** @@ -1567,7 +1588,7 @@ public boolean hasLeft() { * optional double left = 1; * @return this */ - public ProtobufDifferentialDriveWheelPositions clearLeft() { + public ProtobufDifferentialDriveWheelAccelerations clearLeft() { bitField0_ &= ~0x00000001; left = 0D; return this; @@ -1586,7 +1607,7 @@ public double getLeft() { * @param value the left to set * @return this */ - public ProtobufDifferentialDriveWheelPositions setLeft(final double value) { + public ProtobufDifferentialDriveWheelAccelerations setLeft(final double value) { bitField0_ |= 0x00000001; left = value; return this; @@ -1604,7 +1625,7 @@ public boolean hasRight() { * optional double right = 2; * @return this */ - public ProtobufDifferentialDriveWheelPositions clearRight() { + public ProtobufDifferentialDriveWheelAccelerations clearRight() { bitField0_ &= ~0x00000002; right = 0D; return this; @@ -1623,15 +1644,15 @@ public double getRight() { * @param value the right to set * @return this */ - public ProtobufDifferentialDriveWheelPositions setRight(final double value) { + public ProtobufDifferentialDriveWheelAccelerations setRight(final double value) { bitField0_ |= 0x00000002; right = value; return this; } @Override - public ProtobufDifferentialDriveWheelPositions copyFrom( - final ProtobufDifferentialDriveWheelPositions other) { + public ProtobufDifferentialDriveWheelAccelerations copyFrom( + final ProtobufDifferentialDriveWheelAccelerations other) { cachedSize = other.cachedSize; if ((bitField0_ | other.bitField0_) != 0) { bitField0_ = other.bitField0_; @@ -1642,8 +1663,8 @@ public ProtobufDifferentialDriveWheelPositions copyFrom( } @Override - public ProtobufDifferentialDriveWheelPositions mergeFrom( - final ProtobufDifferentialDriveWheelPositions other) { + public ProtobufDifferentialDriveWheelAccelerations mergeFrom( + final ProtobufDifferentialDriveWheelAccelerations other) { if (other.isEmpty()) { return this; } @@ -1658,7 +1679,7 @@ public ProtobufDifferentialDriveWheelPositions mergeFrom( } @Override - public ProtobufDifferentialDriveWheelPositions clear() { + public ProtobufDifferentialDriveWheelAccelerations clear() { if (isEmpty()) { return this; } @@ -1670,7 +1691,7 @@ public ProtobufDifferentialDriveWheelPositions clear() { } @Override - public ProtobufDifferentialDriveWheelPositions clearQuick() { + public ProtobufDifferentialDriveWheelAccelerations clearQuick() { if (isEmpty()) { return this; } @@ -1684,10 +1705,10 @@ public boolean equals(Object o) { if (o == this) { return true; } - if (!(o instanceof ProtobufDifferentialDriveWheelPositions)) { + if (!(o instanceof ProtobufDifferentialDriveWheelAccelerations)) { return false; } - ProtobufDifferentialDriveWheelPositions other = (ProtobufDifferentialDriveWheelPositions) o; + ProtobufDifferentialDriveWheelAccelerations other = (ProtobufDifferentialDriveWheelAccelerations) o; return bitField0_ == other.bitField0_ && (!hasLeft() || ProtoUtil.isEqual(left, other.left)) && (!hasRight() || ProtoUtil.isEqual(right, other.right)); @@ -1719,7 +1740,7 @@ protected int computeSerializedSize() { @Override @SuppressWarnings("fallthrough") - public ProtobufDifferentialDriveWheelPositions mergeFrom(final ProtoSource input) throws + public ProtobufDifferentialDriveWheelAccelerations mergeFrom(final ProtoSource input) throws IOException { // Enabled Fall-Through Optimization (QuickBuffers) int tag = input.readTag(); @@ -1770,7 +1791,7 @@ public void writeTo(final JsonSink output) throws IOException { } @Override - public ProtobufDifferentialDriveWheelPositions mergeFrom(final JsonSource input) throws + public ProtobufDifferentialDriveWheelAccelerations mergeFrom(final JsonSource input) throws IOException { if (!input.beginObject()) { return this; @@ -1810,8 +1831,8 @@ public ProtobufDifferentialDriveWheelPositions mergeFrom(final JsonSource input) } @Override - public ProtobufDifferentialDriveWheelPositions clone() { - return new ProtobufDifferentialDriveWheelPositions().copyFrom(this); + public ProtobufDifferentialDriveWheelAccelerations clone() { + return new ProtobufDifferentialDriveWheelAccelerations().copyFrom(this); } @Override @@ -1819,41 +1840,41 @@ public boolean isEmpty() { return ((bitField0_) == 0); } - public static ProtobufDifferentialDriveWheelPositions parseFrom(final byte[] data) throws + public static ProtobufDifferentialDriveWheelAccelerations parseFrom(final byte[] data) throws InvalidProtocolBufferException { - return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), data).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelAccelerations(), data).checkInitialized(); } - public static ProtobufDifferentialDriveWheelPositions parseFrom(final ProtoSource input) throws - IOException { - return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), input).checkInitialized(); + public static ProtobufDifferentialDriveWheelAccelerations parseFrom(final ProtoSource input) + throws IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelAccelerations(), input).checkInitialized(); } - public static ProtobufDifferentialDriveWheelPositions parseFrom(final JsonSource input) throws - IOException { - return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), input).checkInitialized(); + public static ProtobufDifferentialDriveWheelAccelerations parseFrom(final JsonSource input) + throws IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelAccelerations(), input).checkInitialized(); } /** - * @return factory for creating ProtobufDifferentialDriveWheelPositions messages + * @return factory for creating ProtobufDifferentialDriveWheelAccelerations messages */ - public static MessageFactory getFactory() { - return ProtobufDifferentialDriveWheelPositionsFactory.INSTANCE; + public static MessageFactory getFactory() { + return ProtobufDifferentialDriveWheelAccelerationsFactory.INSTANCE; } /** * @return this type's descriptor. */ public static Descriptors.Descriptor getDescriptor() { - return Kinematics.wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor; + return Kinematics.wpi_proto_ProtobufDifferentialDriveWheelAccelerations_descriptor; } - private enum ProtobufDifferentialDriveWheelPositionsFactory implements MessageFactory { + private enum ProtobufDifferentialDriveWheelAccelerationsFactory implements MessageFactory { INSTANCE; @Override - public ProtobufDifferentialDriveWheelPositions create() { - return ProtobufDifferentialDriveWheelPositions.newInstance(); + public ProtobufDifferentialDriveWheelAccelerations create() { + return ProtobufDifferentialDriveWheelAccelerations.newInstance(); } } @@ -1868,249 +1889,413 @@ static class FieldNames { } /** - * Protobuf type {@code ProtobufMecanumDriveKinematics} + * Protobuf type {@code ProtobufDifferentialDriveWheelPositions} */ - public static final class ProtobufMecanumDriveKinematics extends ProtoMessage implements Cloneable { + public static final class ProtobufDifferentialDriveWheelPositions extends ProtoMessage implements Cloneable { private static final long serialVersionUID = 0L; /** - * optional .wpi.proto.ProtobufTranslation2d front_left = 1; - */ - private final Geometry2D.ProtobufTranslation2d frontLeft = Geometry2D.ProtobufTranslation2d.newInstance(); - - /** - * optional .wpi.proto.ProtobufTranslation2d front_right = 2; - */ - private final Geometry2D.ProtobufTranslation2d frontRight = Geometry2D.ProtobufTranslation2d.newInstance(); - - /** - * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; + * optional double left = 1; */ - private final Geometry2D.ProtobufTranslation2d rearLeft = Geometry2D.ProtobufTranslation2d.newInstance(); + private double left; /** - * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; + * optional double right = 2; */ - private final Geometry2D.ProtobufTranslation2d rearRight = Geometry2D.ProtobufTranslation2d.newInstance(); + private double right; - private ProtobufMecanumDriveKinematics() { + private ProtobufDifferentialDriveWheelPositions() { } /** - * @return a new empty instance of {@code ProtobufMecanumDriveKinematics} + * @return a new empty instance of {@code ProtobufDifferentialDriveWheelPositions} */ - public static ProtobufMecanumDriveKinematics newInstance() { - return new ProtobufMecanumDriveKinematics(); + public static ProtobufDifferentialDriveWheelPositions newInstance() { + return new ProtobufDifferentialDriveWheelPositions(); } /** - * optional .wpi.proto.ProtobufTranslation2d front_left = 1; - * @return whether the frontLeft field is set + * optional double left = 1; + * @return whether the left field is set */ - public boolean hasFrontLeft() { + public boolean hasLeft() { return (bitField0_ & 0x00000001) != 0; } /** - * optional .wpi.proto.ProtobufTranslation2d front_left = 1; + * optional double left = 1; * @return this */ - public ProtobufMecanumDriveKinematics clearFrontLeft() { + public ProtobufDifferentialDriveWheelPositions clearLeft() { bitField0_ &= ~0x00000001; - frontLeft.clear(); + left = 0D; return this; } /** - * optional .wpi.proto.ProtobufTranslation2d front_left = 1; - * - * This method returns the internal storage object without modifying any has state. - * The returned object should not be modified and be treated as read-only. - * - * Use {@link #getMutableFrontLeft()} if you want to modify it. - * - * @return internal storage object for reading - */ - public Geometry2D.ProtobufTranslation2d getFrontLeft() { - return frontLeft; - } - - /** - * optional .wpi.proto.ProtobufTranslation2d front_left = 1; - * - * This method returns the internal storage object and sets the corresponding - * has state. The returned object will become part of this message and its - * contents may be modified as long as the has state is not cleared. - * - * @return internal storage object for modifications + * optional double left = 1; + * @return the left */ - public Geometry2D.ProtobufTranslation2d getMutableFrontLeft() { - bitField0_ |= 0x00000001; - return frontLeft; + public double getLeft() { + return left; } /** - * optional .wpi.proto.ProtobufTranslation2d front_left = 1; - * @param value the frontLeft to set + * optional double left = 1; + * @param value the left to set * @return this */ - public ProtobufMecanumDriveKinematics setFrontLeft( - final Geometry2D.ProtobufTranslation2d value) { + public ProtobufDifferentialDriveWheelPositions setLeft(final double value) { bitField0_ |= 0x00000001; - frontLeft.copyFrom(value); + left = value; return this; } /** - * optional .wpi.proto.ProtobufTranslation2d front_right = 2; - * @return whether the frontRight field is set + * optional double right = 2; + * @return whether the right field is set */ - public boolean hasFrontRight() { + public boolean hasRight() { return (bitField0_ & 0x00000002) != 0; } /** - * optional .wpi.proto.ProtobufTranslation2d front_right = 2; + * optional double right = 2; * @return this */ - public ProtobufMecanumDriveKinematics clearFrontRight() { + public ProtobufDifferentialDriveWheelPositions clearRight() { bitField0_ &= ~0x00000002; - frontRight.clear(); + right = 0D; return this; } /** - * optional .wpi.proto.ProtobufTranslation2d front_right = 2; - * - * This method returns the internal storage object without modifying any has state. - * The returned object should not be modified and be treated as read-only. - * - * Use {@link #getMutableFrontRight()} if you want to modify it. - * - * @return internal storage object for reading - */ - public Geometry2D.ProtobufTranslation2d getFrontRight() { - return frontRight; - } - - /** - * optional .wpi.proto.ProtobufTranslation2d front_right = 2; - * - * This method returns the internal storage object and sets the corresponding - * has state. The returned object will become part of this message and its - * contents may be modified as long as the has state is not cleared. - * - * @return internal storage object for modifications + * optional double right = 2; + * @return the right */ - public Geometry2D.ProtobufTranslation2d getMutableFrontRight() { - bitField0_ |= 0x00000002; - return frontRight; + public double getRight() { + return right; } /** - * optional .wpi.proto.ProtobufTranslation2d front_right = 2; - * @param value the frontRight to set + * optional double right = 2; + * @param value the right to set * @return this */ - public ProtobufMecanumDriveKinematics setFrontRight( - final Geometry2D.ProtobufTranslation2d value) { + public ProtobufDifferentialDriveWheelPositions setRight(final double value) { bitField0_ |= 0x00000002; - frontRight.copyFrom(value); + right = value; return this; } - /** - * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; - * @return whether the rearLeft field is set - */ - public boolean hasRearLeft() { - return (bitField0_ & 0x00000004) != 0; - } - - /** - * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; - * @return this - */ - public ProtobufMecanumDriveKinematics clearRearLeft() { - bitField0_ &= ~0x00000004; - rearLeft.clear(); + @Override + public ProtobufDifferentialDriveWheelPositions copyFrom( + final ProtobufDifferentialDriveWheelPositions other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + left = other.left; + right = other.right; + } return this; } - /** - * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; - * - * This method returns the internal storage object without modifying any has state. - * The returned object should not be modified and be treated as read-only. - * - * Use {@link #getMutableRearLeft()} if you want to modify it. - * - * @return internal storage object for reading - */ - public Geometry2D.ProtobufTranslation2d getRearLeft() { - return rearLeft; + @Override + public ProtobufDifferentialDriveWheelPositions mergeFrom( + final ProtobufDifferentialDriveWheelPositions other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasLeft()) { + setLeft(other.left); + } + if (other.hasRight()) { + setRight(other.right); + } + return this; } - /** - * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; - * - * This method returns the internal storage object and sets the corresponding - * has state. The returned object will become part of this message and its - * contents may be modified as long as the has state is not cleared. - * - * @return internal storage object for modifications - */ - public Geometry2D.ProtobufTranslation2d getMutableRearLeft() { - bitField0_ |= 0x00000004; - return rearLeft; + @Override + public ProtobufDifferentialDriveWheelPositions clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + left = 0D; + right = 0D; + return this; } - /** - * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; - * @param value the rearLeft to set - * @return this - */ - public ProtobufMecanumDriveKinematics setRearLeft( - final Geometry2D.ProtobufTranslation2d value) { - bitField0_ |= 0x00000004; - rearLeft.copyFrom(value); + @Override + public ProtobufDifferentialDriveWheelPositions clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; return this; } + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufDifferentialDriveWheelPositions)) { + return false; + } + ProtobufDifferentialDriveWheelPositions other = (ProtobufDifferentialDriveWheelPositions) o; + return bitField0_ == other.bitField0_ + && (!hasLeft() || ProtoUtil.isEqual(left, other.left)) + && (!hasRight() || ProtoUtil.isEqual(right, other.right)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 9); + output.writeDoubleNoTag(left); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(right); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += 9; + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufDifferentialDriveWheelPositions mergeFrom(final ProtoSource input) throws + IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 9: { + // left + left = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 17) { + break; + } + } + case 17: { + // right + right = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.left, left); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.right, right); + } + output.endObject(); + } + + @Override + public ProtobufDifferentialDriveWheelPositions mergeFrom(final JsonSource input) throws + IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 3317767: { + if (input.isAtField(FieldNames.left)) { + if (!input.trySkipNullValue()) { + left = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case 108511772: { + if (input.isAtField(FieldNames.right)) { + if (!input.trySkipNullValue()) { + right = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions clone() { + return new ProtobufDifferentialDriveWheelPositions().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufDifferentialDriveWheelPositions parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), data).checkInitialized(); + } + + public static ProtobufDifferentialDriveWheelPositions parseFrom(final ProtoSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), input).checkInitialized(); + } + + public static ProtobufDifferentialDriveWheelPositions parseFrom(final JsonSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), input).checkInitialized(); + } + /** - * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; - * @return whether the rearRight field is set + * @return factory for creating ProtobufDifferentialDriveWheelPositions messages */ - public boolean hasRearRight() { - return (bitField0_ & 0x00000008) != 0; + public static MessageFactory getFactory() { + return ProtobufDifferentialDriveWheelPositionsFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Kinematics.wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor; + } + + private enum ProtobufDifferentialDriveWheelPositionsFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufDifferentialDriveWheelPositions create() { + return ProtobufDifferentialDriveWheelPositions.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName left = FieldName.forField("left"); + + static final FieldName right = FieldName.forField("right"); } + } + + /** + * Protobuf type {@code ProtobufMecanumDriveKinematics} + */ + public static final class ProtobufMecanumDriveKinematics extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional .wpi.proto.ProtobufTranslation2d front_left = 1; + */ + private final Geometry2D.ProtobufTranslation2d frontLeft = Geometry2D.ProtobufTranslation2d.newInstance(); + + /** + * optional .wpi.proto.ProtobufTranslation2d front_right = 2; + */ + private final Geometry2D.ProtobufTranslation2d frontRight = Geometry2D.ProtobufTranslation2d.newInstance(); + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; + */ + private final Geometry2D.ProtobufTranslation2d rearLeft = Geometry2D.ProtobufTranslation2d.newInstance(); /** * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; + */ + private final Geometry2D.ProtobufTranslation2d rearRight = Geometry2D.ProtobufTranslation2d.newInstance(); + + private ProtobufMecanumDriveKinematics() { + } + + /** + * @return a new empty instance of {@code ProtobufMecanumDriveKinematics} + */ + public static ProtobufMecanumDriveKinematics newInstance() { + return new ProtobufMecanumDriveKinematics(); + } + + /** + * optional .wpi.proto.ProtobufTranslation2d front_left = 1; + * @return whether the frontLeft field is set + */ + public boolean hasFrontLeft() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d front_left = 1; * @return this */ - public ProtobufMecanumDriveKinematics clearRearRight() { - bitField0_ &= ~0x00000008; - rearRight.clear(); + public ProtobufMecanumDriveKinematics clearFrontLeft() { + bitField0_ &= ~0x00000001; + frontLeft.clear(); return this; } /** - * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; + * optional .wpi.proto.ProtobufTranslation2d front_left = 1; * * This method returns the internal storage object without modifying any has state. * The returned object should not be modified and be treated as read-only. * - * Use {@link #getMutableRearRight()} if you want to modify it. + * Use {@link #getMutableFrontLeft()} if you want to modify it. * * @return internal storage object for reading */ - public Geometry2D.ProtobufTranslation2d getRearRight() { - return rearRight; + public Geometry2D.ProtobufTranslation2d getFrontLeft() { + return frontLeft; } /** - * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; + * optional .wpi.proto.ProtobufTranslation2d front_left = 1; * * This method returns the internal storage object and sets the corresponding * has state. The returned object will become part of this message and its @@ -2118,82 +2303,756 @@ public Geometry2D.ProtobufTranslation2d getRearRight() { * * @return internal storage object for modifications */ - public Geometry2D.ProtobufTranslation2d getMutableRearRight() { - bitField0_ |= 0x00000008; - return rearRight; + public Geometry2D.ProtobufTranslation2d getMutableFrontLeft() { + bitField0_ |= 0x00000001; + return frontLeft; } /** - * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; - * @param value the rearRight to set + * optional .wpi.proto.ProtobufTranslation2d front_left = 1; + * @param value the frontLeft to set + * @return this + */ + public ProtobufMecanumDriveKinematics setFrontLeft( + final Geometry2D.ProtobufTranslation2d value) { + bitField0_ |= 0x00000001; + frontLeft.copyFrom(value); + return this; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d front_right = 2; + * @return whether the frontRight field is set + */ + public boolean hasFrontRight() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d front_right = 2; + * @return this + */ + public ProtobufMecanumDriveKinematics clearFrontRight() { + bitField0_ &= ~0x00000002; + frontRight.clear(); + return this; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d front_right = 2; + * + * This method returns the internal storage object without modifying any has state. + * The returned object should not be modified and be treated as read-only. + * + * Use {@link #getMutableFrontRight()} if you want to modify it. + * + * @return internal storage object for reading + */ + public Geometry2D.ProtobufTranslation2d getFrontRight() { + return frontRight; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d front_right = 2; + * + * This method returns the internal storage object and sets the corresponding + * has state. The returned object will become part of this message and its + * contents may be modified as long as the has state is not cleared. + * + * @return internal storage object for modifications + */ + public Geometry2D.ProtobufTranslation2d getMutableFrontRight() { + bitField0_ |= 0x00000002; + return frontRight; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d front_right = 2; + * @param value the frontRight to set + * @return this + */ + public ProtobufMecanumDriveKinematics setFrontRight( + final Geometry2D.ProtobufTranslation2d value) { + bitField0_ |= 0x00000002; + frontRight.copyFrom(value); + return this; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; + * @return whether the rearLeft field is set + */ + public boolean hasRearLeft() { + return (bitField0_ & 0x00000004) != 0; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; + * @return this + */ + public ProtobufMecanumDriveKinematics clearRearLeft() { + bitField0_ &= ~0x00000004; + rearLeft.clear(); + return this; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; + * + * This method returns the internal storage object without modifying any has state. + * The returned object should not be modified and be treated as read-only. + * + * Use {@link #getMutableRearLeft()} if you want to modify it. + * + * @return internal storage object for reading + */ + public Geometry2D.ProtobufTranslation2d getRearLeft() { + return rearLeft; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; + * + * This method returns the internal storage object and sets the corresponding + * has state. The returned object will become part of this message and its + * contents may be modified as long as the has state is not cleared. + * + * @return internal storage object for modifications + */ + public Geometry2D.ProtobufTranslation2d getMutableRearLeft() { + bitField0_ |= 0x00000004; + return rearLeft; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_left = 3; + * @param value the rearLeft to set + * @return this + */ + public ProtobufMecanumDriveKinematics setRearLeft( + final Geometry2D.ProtobufTranslation2d value) { + bitField0_ |= 0x00000004; + rearLeft.copyFrom(value); + return this; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; + * @return whether the rearRight field is set + */ + public boolean hasRearRight() { + return (bitField0_ & 0x00000008) != 0; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; + * @return this + */ + public ProtobufMecanumDriveKinematics clearRearRight() { + bitField0_ &= ~0x00000008; + rearRight.clear(); + return this; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; + * + * This method returns the internal storage object without modifying any has state. + * The returned object should not be modified and be treated as read-only. + * + * Use {@link #getMutableRearRight()} if you want to modify it. + * + * @return internal storage object for reading + */ + public Geometry2D.ProtobufTranslation2d getRearRight() { + return rearRight; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; + * + * This method returns the internal storage object and sets the corresponding + * has state. The returned object will become part of this message and its + * contents may be modified as long as the has state is not cleared. + * + * @return internal storage object for modifications + */ + public Geometry2D.ProtobufTranslation2d getMutableRearRight() { + bitField0_ |= 0x00000008; + return rearRight; + } + + /** + * optional .wpi.proto.ProtobufTranslation2d rear_right = 4; + * @param value the rearRight to set * @return this */ public ProtobufMecanumDriveKinematics setRearRight( final Geometry2D.ProtobufTranslation2d value) { bitField0_ |= 0x00000008; - rearRight.copyFrom(value); + rearRight.copyFrom(value); + return this; + } + + @Override + public ProtobufMecanumDriveKinematics copyFrom(final ProtobufMecanumDriveKinematics other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + frontLeft.copyFrom(other.frontLeft); + frontRight.copyFrom(other.frontRight); + rearLeft.copyFrom(other.rearLeft); + rearRight.copyFrom(other.rearRight); + } + return this; + } + + @Override + public ProtobufMecanumDriveKinematics mergeFrom(final ProtobufMecanumDriveKinematics other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasFrontLeft()) { + getMutableFrontLeft().mergeFrom(other.frontLeft); + } + if (other.hasFrontRight()) { + getMutableFrontRight().mergeFrom(other.frontRight); + } + if (other.hasRearLeft()) { + getMutableRearLeft().mergeFrom(other.rearLeft); + } + if (other.hasRearRight()) { + getMutableRearRight().mergeFrom(other.rearRight); + } + return this; + } + + @Override + public ProtobufMecanumDriveKinematics clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + frontLeft.clear(); + frontRight.clear(); + rearLeft.clear(); + rearRight.clear(); + return this; + } + + @Override + public ProtobufMecanumDriveKinematics clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + frontLeft.clearQuick(); + frontRight.clearQuick(); + rearLeft.clearQuick(); + rearRight.clearQuick(); + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufMecanumDriveKinematics)) { + return false; + } + ProtobufMecanumDriveKinematics other = (ProtobufMecanumDriveKinematics) o; + return bitField0_ == other.bitField0_ + && (!hasFrontLeft() || frontLeft.equals(other.frontLeft)) + && (!hasFrontRight() || frontRight.equals(other.frontRight)) + && (!hasRearLeft() || rearLeft.equals(other.rearLeft)) + && (!hasRearRight() || rearRight.equals(other.rearRight)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 10); + output.writeMessageNoTag(frontLeft); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 18); + output.writeMessageNoTag(frontRight); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeRawByte((byte) 26); + output.writeMessageNoTag(rearLeft); + } + if ((bitField0_ & 0x00000008) != 0) { + output.writeRawByte((byte) 34); + output.writeMessageNoTag(rearRight); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 1 + ProtoSink.computeMessageSizeNoTag(frontLeft); + } + if ((bitField0_ & 0x00000002) != 0) { + size += 1 + ProtoSink.computeMessageSizeNoTag(frontRight); + } + if ((bitField0_ & 0x00000004) != 0) { + size += 1 + ProtoSink.computeMessageSizeNoTag(rearLeft); + } + if ((bitField0_ & 0x00000008) != 0) { + size += 1 + ProtoSink.computeMessageSizeNoTag(rearRight); + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufMecanumDriveKinematics mergeFrom(final ProtoSource input) throws IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 10: { + // frontLeft + input.readMessage(frontLeft); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 18) { + break; + } + } + case 18: { + // frontRight + input.readMessage(frontRight); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 26) { + break; + } + } + case 26: { + // rearLeft + input.readMessage(rearLeft); + bitField0_ |= 0x00000004; + tag = input.readTag(); + if (tag != 34) { + break; + } + } + case 34: { + // rearRight + input.readMessage(rearRight); + bitField0_ |= 0x00000008; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeMessage(FieldNames.frontLeft, frontLeft); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeMessage(FieldNames.frontRight, frontRight); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeMessage(FieldNames.rearLeft, rearLeft); + } + if ((bitField0_ & 0x00000008) != 0) { + output.writeMessage(FieldNames.rearRight, rearRight); + } + output.endObject(); + } + + @Override + public ProtobufMecanumDriveKinematics mergeFrom(final JsonSource input) throws IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 127514064: + case -324277155: { + if (input.isAtField(FieldNames.frontLeft)) { + if (!input.trySkipNullValue()) { + input.readMessage(frontLeft); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case -336370317: + case -1456996218: { + if (input.isAtField(FieldNames.frontRight)) { + if (!input.trySkipNullValue()) { + input.readMessage(frontRight); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + case -854852661: + case -712874558: { + if (input.isAtField(FieldNames.rearLeft)) { + if (!input.trySkipNullValue()) { + input.readMessage(rearLeft); + bitField0_ |= 0x00000004; + } + } else { + input.skipUnknownField(); + } + break; + } + case -724967720: + case -618613823: { + if (input.isAtField(FieldNames.rearRight)) { + if (!input.trySkipNullValue()) { + input.readMessage(rearRight); + bitField0_ |= 0x00000008; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufMecanumDriveKinematics clone() { + return new ProtobufMecanumDriveKinematics().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufMecanumDriveKinematics parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveKinematics(), data).checkInitialized(); + } + + public static ProtobufMecanumDriveKinematics parseFrom(final ProtoSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveKinematics(), input).checkInitialized(); + } + + public static ProtobufMecanumDriveKinematics parseFrom(final JsonSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveKinematics(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufMecanumDriveKinematics messages + */ + public static MessageFactory getFactory() { + return ProtobufMecanumDriveKinematicsFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Kinematics.wpi_proto_ProtobufMecanumDriveKinematics_descriptor; + } + + private enum ProtobufMecanumDriveKinematicsFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufMecanumDriveKinematics create() { + return ProtobufMecanumDriveKinematics.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName frontLeft = FieldName.forField("frontLeft", "front_left"); + + static final FieldName frontRight = FieldName.forField("frontRight", "front_right"); + + static final FieldName rearLeft = FieldName.forField("rearLeft", "rear_left"); + + static final FieldName rearRight = FieldName.forField("rearRight", "rear_right"); + } + } + + /** + * Protobuf type {@code ProtobufMecanumDriveWheelPositions} + */ + public static final class ProtobufMecanumDriveWheelPositions extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double front_left = 1; + */ + private double frontLeft; + + /** + * optional double front_right = 2; + */ + private double frontRight; + + /** + * optional double rear_left = 3; + */ + private double rearLeft; + + /** + * optional double rear_right = 4; + */ + private double rearRight; + + private ProtobufMecanumDriveWheelPositions() { + } + + /** + * @return a new empty instance of {@code ProtobufMecanumDriveWheelPositions} + */ + public static ProtobufMecanumDriveWheelPositions newInstance() { + return new ProtobufMecanumDriveWheelPositions(); + } + + /** + * optional double front_left = 1; + * @return whether the frontLeft field is set + */ + public boolean hasFrontLeft() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double front_left = 1; + * @return this + */ + public ProtobufMecanumDriveWheelPositions clearFrontLeft() { + bitField0_ &= ~0x00000001; + frontLeft = 0D; + return this; + } + + /** + * optional double front_left = 1; + * @return the frontLeft + */ + public double getFrontLeft() { + return frontLeft; + } + + /** + * optional double front_left = 1; + * @param value the frontLeft to set + * @return this + */ + public ProtobufMecanumDriveWheelPositions setFrontLeft(final double value) { + bitField0_ |= 0x00000001; + frontLeft = value; + return this; + } + + /** + * optional double front_right = 2; + * @return whether the frontRight field is set + */ + public boolean hasFrontRight() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double front_right = 2; + * @return this + */ + public ProtobufMecanumDriveWheelPositions clearFrontRight() { + bitField0_ &= ~0x00000002; + frontRight = 0D; + return this; + } + + /** + * optional double front_right = 2; + * @return the frontRight + */ + public double getFrontRight() { + return frontRight; + } + + /** + * optional double front_right = 2; + * @param value the frontRight to set + * @return this + */ + public ProtobufMecanumDriveWheelPositions setFrontRight(final double value) { + bitField0_ |= 0x00000002; + frontRight = value; + return this; + } + + /** + * optional double rear_left = 3; + * @return whether the rearLeft field is set + */ + public boolean hasRearLeft() { + return (bitField0_ & 0x00000004) != 0; + } + + /** + * optional double rear_left = 3; + * @return this + */ + public ProtobufMecanumDriveWheelPositions clearRearLeft() { + bitField0_ &= ~0x00000004; + rearLeft = 0D; + return this; + } + + /** + * optional double rear_left = 3; + * @return the rearLeft + */ + public double getRearLeft() { + return rearLeft; + } + + /** + * optional double rear_left = 3; + * @param value the rearLeft to set + * @return this + */ + public ProtobufMecanumDriveWheelPositions setRearLeft(final double value) { + bitField0_ |= 0x00000004; + rearLeft = value; + return this; + } + + /** + * optional double rear_right = 4; + * @return whether the rearRight field is set + */ + public boolean hasRearRight() { + return (bitField0_ & 0x00000008) != 0; + } + + /** + * optional double rear_right = 4; + * @return this + */ + public ProtobufMecanumDriveWheelPositions clearRearRight() { + bitField0_ &= ~0x00000008; + rearRight = 0D; + return this; + } + + /** + * optional double rear_right = 4; + * @return the rearRight + */ + public double getRearRight() { + return rearRight; + } + + /** + * optional double rear_right = 4; + * @param value the rearRight to set + * @return this + */ + public ProtobufMecanumDriveWheelPositions setRearRight(final double value) { + bitField0_ |= 0x00000008; + rearRight = value; return this; } @Override - public ProtobufMecanumDriveKinematics copyFrom(final ProtobufMecanumDriveKinematics other) { + public ProtobufMecanumDriveWheelPositions copyFrom( + final ProtobufMecanumDriveWheelPositions other) { cachedSize = other.cachedSize; if ((bitField0_ | other.bitField0_) != 0) { bitField0_ = other.bitField0_; - frontLeft.copyFrom(other.frontLeft); - frontRight.copyFrom(other.frontRight); - rearLeft.copyFrom(other.rearLeft); - rearRight.copyFrom(other.rearRight); + frontLeft = other.frontLeft; + frontRight = other.frontRight; + rearLeft = other.rearLeft; + rearRight = other.rearRight; } return this; } @Override - public ProtobufMecanumDriveKinematics mergeFrom(final ProtobufMecanumDriveKinematics other) { + public ProtobufMecanumDriveWheelPositions mergeFrom( + final ProtobufMecanumDriveWheelPositions other) { if (other.isEmpty()) { return this; } cachedSize = -1; if (other.hasFrontLeft()) { - getMutableFrontLeft().mergeFrom(other.frontLeft); + setFrontLeft(other.frontLeft); } if (other.hasFrontRight()) { - getMutableFrontRight().mergeFrom(other.frontRight); + setFrontRight(other.frontRight); } if (other.hasRearLeft()) { - getMutableRearLeft().mergeFrom(other.rearLeft); + setRearLeft(other.rearLeft); } if (other.hasRearRight()) { - getMutableRearRight().mergeFrom(other.rearRight); + setRearRight(other.rearRight); } return this; } @Override - public ProtobufMecanumDriveKinematics clear() { + public ProtobufMecanumDriveWheelPositions clear() { if (isEmpty()) { return this; } cachedSize = -1; bitField0_ = 0; - frontLeft.clear(); - frontRight.clear(); - rearLeft.clear(); - rearRight.clear(); + frontLeft = 0D; + frontRight = 0D; + rearLeft = 0D; + rearRight = 0D; return this; } @Override - public ProtobufMecanumDriveKinematics clearQuick() { + public ProtobufMecanumDriveWheelPositions clearQuick() { if (isEmpty()) { return this; } cachedSize = -1; bitField0_ = 0; - frontLeft.clearQuick(); - frontRight.clearQuick(); - rearLeft.clearQuick(); - rearRight.clearQuick(); return this; } @@ -2202,34 +3061,34 @@ public boolean equals(Object o) { if (o == this) { return true; } - if (!(o instanceof ProtobufMecanumDriveKinematics)) { + if (!(o instanceof ProtobufMecanumDriveWheelPositions)) { return false; } - ProtobufMecanumDriveKinematics other = (ProtobufMecanumDriveKinematics) o; + ProtobufMecanumDriveWheelPositions other = (ProtobufMecanumDriveWheelPositions) o; return bitField0_ == other.bitField0_ - && (!hasFrontLeft() || frontLeft.equals(other.frontLeft)) - && (!hasFrontRight() || frontRight.equals(other.frontRight)) - && (!hasRearLeft() || rearLeft.equals(other.rearLeft)) - && (!hasRearRight() || rearRight.equals(other.rearRight)); + && (!hasFrontLeft() || ProtoUtil.isEqual(frontLeft, other.frontLeft)) + && (!hasFrontRight() || ProtoUtil.isEqual(frontRight, other.frontRight)) + && (!hasRearLeft() || ProtoUtil.isEqual(rearLeft, other.rearLeft)) + && (!hasRearRight() || ProtoUtil.isEqual(rearRight, other.rearRight)); } @Override public void writeTo(final ProtoSink output) throws IOException { if ((bitField0_ & 0x00000001) != 0) { - output.writeRawByte((byte) 10); - output.writeMessageNoTag(frontLeft); + output.writeRawByte((byte) 9); + output.writeDoubleNoTag(frontLeft); } if ((bitField0_ & 0x00000002) != 0) { - output.writeRawByte((byte) 18); - output.writeMessageNoTag(frontRight); + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(frontRight); } if ((bitField0_ & 0x00000004) != 0) { - output.writeRawByte((byte) 26); - output.writeMessageNoTag(rearLeft); + output.writeRawByte((byte) 25); + output.writeDoubleNoTag(rearLeft); } if ((bitField0_ & 0x00000008) != 0) { - output.writeRawByte((byte) 34); - output.writeMessageNoTag(rearRight); + output.writeRawByte((byte) 33); + output.writeDoubleNoTag(rearRight); } } @@ -2237,57 +3096,58 @@ public void writeTo(final ProtoSink output) throws IOException { protected int computeSerializedSize() { int size = 0; if ((bitField0_ & 0x00000001) != 0) { - size += 1 + ProtoSink.computeMessageSizeNoTag(frontLeft); + size += 9; } if ((bitField0_ & 0x00000002) != 0) { - size += 1 + ProtoSink.computeMessageSizeNoTag(frontRight); + size += 9; } if ((bitField0_ & 0x00000004) != 0) { - size += 1 + ProtoSink.computeMessageSizeNoTag(rearLeft); + size += 9; } if ((bitField0_ & 0x00000008) != 0) { - size += 1 + ProtoSink.computeMessageSizeNoTag(rearRight); + size += 9; } return size; } @Override @SuppressWarnings("fallthrough") - public ProtobufMecanumDriveKinematics mergeFrom(final ProtoSource input) throws IOException { + public ProtobufMecanumDriveWheelPositions mergeFrom(final ProtoSource input) throws + IOException { // Enabled Fall-Through Optimization (QuickBuffers) int tag = input.readTag(); while (true) { switch (tag) { - case 10: { + case 9: { // frontLeft - input.readMessage(frontLeft); + frontLeft = input.readDouble(); bitField0_ |= 0x00000001; tag = input.readTag(); - if (tag != 18) { + if (tag != 17) { break; } } - case 18: { + case 17: { // frontRight - input.readMessage(frontRight); + frontRight = input.readDouble(); bitField0_ |= 0x00000002; tag = input.readTag(); - if (tag != 26) { + if (tag != 25) { break; } } - case 26: { + case 25: { // rearLeft - input.readMessage(rearLeft); + rearLeft = input.readDouble(); bitField0_ |= 0x00000004; tag = input.readTag(); - if (tag != 34) { + if (tag != 33) { break; } } - case 34: { + case 33: { // rearRight - input.readMessage(rearRight); + rearRight = input.readDouble(); bitField0_ |= 0x00000008; tag = input.readTag(); if (tag != 0) { @@ -2312,22 +3172,22 @@ public ProtobufMecanumDriveKinematics mergeFrom(final ProtoSource input) throws public void writeTo(final JsonSink output) throws IOException { output.beginObject(); if ((bitField0_ & 0x00000001) != 0) { - output.writeMessage(FieldNames.frontLeft, frontLeft); + output.writeDouble(FieldNames.frontLeft, frontLeft); } if ((bitField0_ & 0x00000002) != 0) { - output.writeMessage(FieldNames.frontRight, frontRight); + output.writeDouble(FieldNames.frontRight, frontRight); } if ((bitField0_ & 0x00000004) != 0) { - output.writeMessage(FieldNames.rearLeft, rearLeft); + output.writeDouble(FieldNames.rearLeft, rearLeft); } if ((bitField0_ & 0x00000008) != 0) { - output.writeMessage(FieldNames.rearRight, rearRight); + output.writeDouble(FieldNames.rearRight, rearRight); } output.endObject(); } @Override - public ProtobufMecanumDriveKinematics mergeFrom(final JsonSource input) throws IOException { + public ProtobufMecanumDriveWheelPositions mergeFrom(final JsonSource input) throws IOException { if (!input.beginObject()) { return this; } @@ -2337,7 +3197,7 @@ public ProtobufMecanumDriveKinematics mergeFrom(final JsonSource input) throws I case -324277155: { if (input.isAtField(FieldNames.frontLeft)) { if (!input.trySkipNullValue()) { - input.readMessage(frontLeft); + frontLeft = input.readDouble(); bitField0_ |= 0x00000001; } } else { @@ -2349,7 +3209,7 @@ public ProtobufMecanumDriveKinematics mergeFrom(final JsonSource input) throws I case -1456996218: { if (input.isAtField(FieldNames.frontRight)) { if (!input.trySkipNullValue()) { - input.readMessage(frontRight); + frontRight = input.readDouble(); bitField0_ |= 0x00000002; } } else { @@ -2361,7 +3221,7 @@ public ProtobufMecanumDriveKinematics mergeFrom(final JsonSource input) throws I case -712874558: { if (input.isAtField(FieldNames.rearLeft)) { if (!input.trySkipNullValue()) { - input.readMessage(rearLeft); + rearLeft = input.readDouble(); bitField0_ |= 0x00000004; } } else { @@ -2373,7 +3233,7 @@ public ProtobufMecanumDriveKinematics mergeFrom(final JsonSource input) throws I case -618613823: { if (input.isAtField(FieldNames.rearRight)) { if (!input.trySkipNullValue()) { - input.readMessage(rearRight); + rearRight = input.readDouble(); bitField0_ |= 0x00000008; } } else { @@ -2392,8 +3252,8 @@ public ProtobufMecanumDriveKinematics mergeFrom(final JsonSource input) throws I } @Override - public ProtobufMecanumDriveKinematics clone() { - return new ProtobufMecanumDriveKinematics().copyFrom(this); + public ProtobufMecanumDriveWheelPositions clone() { + return new ProtobufMecanumDriveWheelPositions().copyFrom(this); } @Override @@ -2401,41 +3261,41 @@ public boolean isEmpty() { return ((bitField0_) == 0); } - public static ProtobufMecanumDriveKinematics parseFrom(final byte[] data) throws + public static ProtobufMecanumDriveWheelPositions parseFrom(final byte[] data) throws InvalidProtocolBufferException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveKinematics(), data).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelPositions(), data).checkInitialized(); } - public static ProtobufMecanumDriveKinematics parseFrom(final ProtoSource input) throws + public static ProtobufMecanumDriveWheelPositions parseFrom(final ProtoSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveKinematics(), input).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelPositions(), input).checkInitialized(); } - public static ProtobufMecanumDriveKinematics parseFrom(final JsonSource input) throws + public static ProtobufMecanumDriveWheelPositions parseFrom(final JsonSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveKinematics(), input).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelPositions(), input).checkInitialized(); } /** - * @return factory for creating ProtobufMecanumDriveKinematics messages + * @return factory for creating ProtobufMecanumDriveWheelPositions messages */ - public static MessageFactory getFactory() { - return ProtobufMecanumDriveKinematicsFactory.INSTANCE; + public static MessageFactory getFactory() { + return ProtobufMecanumDriveWheelPositionsFactory.INSTANCE; } /** * @return this type's descriptor. */ public static Descriptors.Descriptor getDescriptor() { - return Kinematics.wpi_proto_ProtobufMecanumDriveKinematics_descriptor; + return Kinematics.wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor; } - private enum ProtobufMecanumDriveKinematicsFactory implements MessageFactory { + private enum ProtobufMecanumDriveWheelPositionsFactory implements MessageFactory { INSTANCE; @Override - public ProtobufMecanumDriveKinematics create() { - return ProtobufMecanumDriveKinematics.newInstance(); + public ProtobufMecanumDriveWheelPositions create() { + return ProtobufMecanumDriveWheelPositions.newInstance(); } } @@ -2454,9 +3314,9 @@ static class FieldNames { } /** - * Protobuf type {@code ProtobufMecanumDriveWheelPositions} + * Protobuf type {@code ProtobufMecanumDriveWheelSpeeds} */ - public static final class ProtobufMecanumDriveWheelPositions extends ProtoMessage implements Cloneable { + public static final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage implements Cloneable { private static final long serialVersionUID = 0L; /** @@ -2479,14 +3339,14 @@ public static final class ProtobufMecanumDriveWheelPositions extends ProtoMessag */ private double rearRight; - private ProtobufMecanumDriveWheelPositions() { + private ProtobufMecanumDriveWheelSpeeds() { } /** - * @return a new empty instance of {@code ProtobufMecanumDriveWheelPositions} + * @return a new empty instance of {@code ProtobufMecanumDriveWheelSpeeds} */ - public static ProtobufMecanumDriveWheelPositions newInstance() { - return new ProtobufMecanumDriveWheelPositions(); + public static ProtobufMecanumDriveWheelSpeeds newInstance() { + return new ProtobufMecanumDriveWheelSpeeds(); } /** @@ -2501,7 +3361,7 @@ public boolean hasFrontLeft() { * optional double front_left = 1; * @return this */ - public ProtobufMecanumDriveWheelPositions clearFrontLeft() { + public ProtobufMecanumDriveWheelSpeeds clearFrontLeft() { bitField0_ &= ~0x00000001; frontLeft = 0D; return this; @@ -2520,7 +3380,7 @@ public double getFrontLeft() { * @param value the frontLeft to set * @return this */ - public ProtobufMecanumDriveWheelPositions setFrontLeft(final double value) { + public ProtobufMecanumDriveWheelSpeeds setFrontLeft(final double value) { bitField0_ |= 0x00000001; frontLeft = value; return this; @@ -2538,7 +3398,7 @@ public boolean hasFrontRight() { * optional double front_right = 2; * @return this */ - public ProtobufMecanumDriveWheelPositions clearFrontRight() { + public ProtobufMecanumDriveWheelSpeeds clearFrontRight() { bitField0_ &= ~0x00000002; frontRight = 0D; return this; @@ -2557,7 +3417,7 @@ public double getFrontRight() { * @param value the frontRight to set * @return this */ - public ProtobufMecanumDriveWheelPositions setFrontRight(final double value) { + public ProtobufMecanumDriveWheelSpeeds setFrontRight(final double value) { bitField0_ |= 0x00000002; frontRight = value; return this; @@ -2575,7 +3435,7 @@ public boolean hasRearLeft() { * optional double rear_left = 3; * @return this */ - public ProtobufMecanumDriveWheelPositions clearRearLeft() { + public ProtobufMecanumDriveWheelSpeeds clearRearLeft() { bitField0_ &= ~0x00000004; rearLeft = 0D; return this; @@ -2594,7 +3454,7 @@ public double getRearLeft() { * @param value the rearLeft to set * @return this */ - public ProtobufMecanumDriveWheelPositions setRearLeft(final double value) { + public ProtobufMecanumDriveWheelSpeeds setRearLeft(final double value) { bitField0_ |= 0x00000004; rearLeft = value; return this; @@ -2612,7 +3472,7 @@ public boolean hasRearRight() { * optional double rear_right = 4; * @return this */ - public ProtobufMecanumDriveWheelPositions clearRearRight() { + public ProtobufMecanumDriveWheelSpeeds clearRearRight() { bitField0_ &= ~0x00000008; rearRight = 0D; return this; @@ -2631,15 +3491,14 @@ public double getRearRight() { * @param value the rearRight to set * @return this */ - public ProtobufMecanumDriveWheelPositions setRearRight(final double value) { + public ProtobufMecanumDriveWheelSpeeds setRearRight(final double value) { bitField0_ |= 0x00000008; rearRight = value; return this; } @Override - public ProtobufMecanumDriveWheelPositions copyFrom( - final ProtobufMecanumDriveWheelPositions other) { + public ProtobufMecanumDriveWheelSpeeds copyFrom(final ProtobufMecanumDriveWheelSpeeds other) { cachedSize = other.cachedSize; if ((bitField0_ | other.bitField0_) != 0) { bitField0_ = other.bitField0_; @@ -2652,8 +3511,7 @@ public ProtobufMecanumDriveWheelPositions copyFrom( } @Override - public ProtobufMecanumDriveWheelPositions mergeFrom( - final ProtobufMecanumDriveWheelPositions other) { + public ProtobufMecanumDriveWheelSpeeds mergeFrom(final ProtobufMecanumDriveWheelSpeeds other) { if (other.isEmpty()) { return this; } @@ -2674,7 +3532,7 @@ public ProtobufMecanumDriveWheelPositions mergeFrom( } @Override - public ProtobufMecanumDriveWheelPositions clear() { + public ProtobufMecanumDriveWheelSpeeds clear() { if (isEmpty()) { return this; } @@ -2688,7 +3546,7 @@ public ProtobufMecanumDriveWheelPositions clear() { } @Override - public ProtobufMecanumDriveWheelPositions clearQuick() { + public ProtobufMecanumDriveWheelSpeeds clearQuick() { if (isEmpty()) { return this; } @@ -2702,10 +3560,10 @@ public boolean equals(Object o) { if (o == this) { return true; } - if (!(o instanceof ProtobufMecanumDriveWheelPositions)) { + if (!(o instanceof ProtobufMecanumDriveWheelSpeeds)) { return false; } - ProtobufMecanumDriveWheelPositions other = (ProtobufMecanumDriveWheelPositions) o; + ProtobufMecanumDriveWheelSpeeds other = (ProtobufMecanumDriveWheelSpeeds) o; return bitField0_ == other.bitField0_ && (!hasFrontLeft() || ProtoUtil.isEqual(frontLeft, other.frontLeft)) && (!hasFrontRight() || ProtoUtil.isEqual(frontRight, other.frontRight)) @@ -2753,8 +3611,7 @@ protected int computeSerializedSize() { @Override @SuppressWarnings("fallthrough") - public ProtobufMecanumDriveWheelPositions mergeFrom(final ProtoSource input) throws - IOException { + public ProtobufMecanumDriveWheelSpeeds mergeFrom(final ProtoSource input) throws IOException { // Enabled Fall-Through Optimization (QuickBuffers) int tag = input.readTag(); while (true) { @@ -2828,7 +3685,7 @@ public void writeTo(final JsonSink output) throws IOException { } @Override - public ProtobufMecanumDriveWheelPositions mergeFrom(final JsonSource input) throws IOException { + public ProtobufMecanumDriveWheelSpeeds mergeFrom(final JsonSource input) throws IOException { if (!input.beginObject()) { return this; } @@ -2893,8 +3750,8 @@ public ProtobufMecanumDriveWheelPositions mergeFrom(final JsonSource input) thro } @Override - public ProtobufMecanumDriveWheelPositions clone() { - return new ProtobufMecanumDriveWheelPositions().copyFrom(this); + public ProtobufMecanumDriveWheelSpeeds clone() { + return new ProtobufMecanumDriveWheelSpeeds().copyFrom(this); } @Override @@ -2902,41 +3759,41 @@ public boolean isEmpty() { return ((bitField0_) == 0); } - public static ProtobufMecanumDriveWheelPositions parseFrom(final byte[] data) throws + public static ProtobufMecanumDriveWheelSpeeds parseFrom(final byte[] data) throws InvalidProtocolBufferException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelPositions(), data).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), data).checkInitialized(); } - public static ProtobufMecanumDriveWheelPositions parseFrom(final ProtoSource input) throws + public static ProtobufMecanumDriveWheelSpeeds parseFrom(final ProtoSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelPositions(), input).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), input).checkInitialized(); } - public static ProtobufMecanumDriveWheelPositions parseFrom(final JsonSource input) throws + public static ProtobufMecanumDriveWheelSpeeds parseFrom(final JsonSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelPositions(), input).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), input).checkInitialized(); } /** - * @return factory for creating ProtobufMecanumDriveWheelPositions messages + * @return factory for creating ProtobufMecanumDriveWheelSpeeds messages */ - public static MessageFactory getFactory() { - return ProtobufMecanumDriveWheelPositionsFactory.INSTANCE; + public static MessageFactory getFactory() { + return ProtobufMecanumDriveWheelSpeedsFactory.INSTANCE; } /** * @return this type's descriptor. */ public static Descriptors.Descriptor getDescriptor() { - return Kinematics.wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor; + return Kinematics.wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor; } - private enum ProtobufMecanumDriveWheelPositionsFactory implements MessageFactory { + private enum ProtobufMecanumDriveWheelSpeedsFactory implements MessageFactory { INSTANCE; @Override - public ProtobufMecanumDriveWheelPositions create() { - return ProtobufMecanumDriveWheelPositions.newInstance(); + public ProtobufMecanumDriveWheelSpeeds create() { + return ProtobufMecanumDriveWheelSpeeds.newInstance(); } } @@ -2955,9 +3812,9 @@ static class FieldNames { } /** - * Protobuf type {@code ProtobufMecanumDriveWheelSpeeds} + * Protobuf type {@code ProtobufMecanumDriveWheelAccelerations} */ - public static final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage implements Cloneable { + public static final class ProtobufMecanumDriveWheelAccelerations extends ProtoMessage implements Cloneable { private static final long serialVersionUID = 0L; /** @@ -2980,14 +3837,14 @@ public static final class ProtobufMecanumDriveWheelSpeeds extends ProtoMessage

optional double front_left = 1; * @return this */ - public ProtobufMecanumDriveWheelSpeeds clearFrontLeft() { + public ProtobufMecanumDriveWheelAccelerations clearFrontLeft() { bitField0_ &= ~0x00000001; frontLeft = 0D; return this; @@ -3021,7 +3878,7 @@ public double getFrontLeft() { * @param value the frontLeft to set * @return this */ - public ProtobufMecanumDriveWheelSpeeds setFrontLeft(final double value) { + public ProtobufMecanumDriveWheelAccelerations setFrontLeft(final double value) { bitField0_ |= 0x00000001; frontLeft = value; return this; @@ -3039,7 +3896,7 @@ public boolean hasFrontRight() { * optional double front_right = 2; * @return this */ - public ProtobufMecanumDriveWheelSpeeds clearFrontRight() { + public ProtobufMecanumDriveWheelAccelerations clearFrontRight() { bitField0_ &= ~0x00000002; frontRight = 0D; return this; @@ -3058,7 +3915,7 @@ public double getFrontRight() { * @param value the frontRight to set * @return this */ - public ProtobufMecanumDriveWheelSpeeds setFrontRight(final double value) { + public ProtobufMecanumDriveWheelAccelerations setFrontRight(final double value) { bitField0_ |= 0x00000002; frontRight = value; return this; @@ -3076,7 +3933,7 @@ public boolean hasRearLeft() { * optional double rear_left = 3; * @return this */ - public ProtobufMecanumDriveWheelSpeeds clearRearLeft() { + public ProtobufMecanumDriveWheelAccelerations clearRearLeft() { bitField0_ &= ~0x00000004; rearLeft = 0D; return this; @@ -3095,7 +3952,7 @@ public double getRearLeft() { * @param value the rearLeft to set * @return this */ - public ProtobufMecanumDriveWheelSpeeds setRearLeft(final double value) { + public ProtobufMecanumDriveWheelAccelerations setRearLeft(final double value) { bitField0_ |= 0x00000004; rearLeft = value; return this; @@ -3113,7 +3970,7 @@ public boolean hasRearRight() { * optional double rear_right = 4; * @return this */ - public ProtobufMecanumDriveWheelSpeeds clearRearRight() { + public ProtobufMecanumDriveWheelAccelerations clearRearRight() { bitField0_ &= ~0x00000008; rearRight = 0D; return this; @@ -3132,14 +3989,15 @@ public double getRearRight() { * @param value the rearRight to set * @return this */ - public ProtobufMecanumDriveWheelSpeeds setRearRight(final double value) { + public ProtobufMecanumDriveWheelAccelerations setRearRight(final double value) { bitField0_ |= 0x00000008; rearRight = value; return this; } @Override - public ProtobufMecanumDriveWheelSpeeds copyFrom(final ProtobufMecanumDriveWheelSpeeds other) { + public ProtobufMecanumDriveWheelAccelerations copyFrom( + final ProtobufMecanumDriveWheelAccelerations other) { cachedSize = other.cachedSize; if ((bitField0_ | other.bitField0_) != 0) { bitField0_ = other.bitField0_; @@ -3152,7 +4010,8 @@ public ProtobufMecanumDriveWheelSpeeds copyFrom(final ProtobufMecanumDriveWheelS } @Override - public ProtobufMecanumDriveWheelSpeeds mergeFrom(final ProtobufMecanumDriveWheelSpeeds other) { + public ProtobufMecanumDriveWheelAccelerations mergeFrom( + final ProtobufMecanumDriveWheelAccelerations other) { if (other.isEmpty()) { return this; } @@ -3173,7 +4032,7 @@ public ProtobufMecanumDriveWheelSpeeds mergeFrom(final ProtobufMecanumDriveWheel } @Override - public ProtobufMecanumDriveWheelSpeeds clear() { + public ProtobufMecanumDriveWheelAccelerations clear() { if (isEmpty()) { return this; } @@ -3187,7 +4046,7 @@ public ProtobufMecanumDriveWheelSpeeds clear() { } @Override - public ProtobufMecanumDriveWheelSpeeds clearQuick() { + public ProtobufMecanumDriveWheelAccelerations clearQuick() { if (isEmpty()) { return this; } @@ -3201,10 +4060,10 @@ public boolean equals(Object o) { if (o == this) { return true; } - if (!(o instanceof ProtobufMecanumDriveWheelSpeeds)) { + if (!(o instanceof ProtobufMecanumDriveWheelAccelerations)) { return false; } - ProtobufMecanumDriveWheelSpeeds other = (ProtobufMecanumDriveWheelSpeeds) o; + ProtobufMecanumDriveWheelAccelerations other = (ProtobufMecanumDriveWheelAccelerations) o; return bitField0_ == other.bitField0_ && (!hasFrontLeft() || ProtoUtil.isEqual(frontLeft, other.frontLeft)) && (!hasFrontRight() || ProtoUtil.isEqual(frontRight, other.frontRight)) @@ -3252,7 +4111,8 @@ protected int computeSerializedSize() { @Override @SuppressWarnings("fallthrough") - public ProtobufMecanumDriveWheelSpeeds mergeFrom(final ProtoSource input) throws IOException { + public ProtobufMecanumDriveWheelAccelerations mergeFrom(final ProtoSource input) throws + IOException { // Enabled Fall-Through Optimization (QuickBuffers) int tag = input.readTag(); while (true) { @@ -3326,7 +4186,8 @@ public void writeTo(final JsonSink output) throws IOException { } @Override - public ProtobufMecanumDriveWheelSpeeds mergeFrom(final JsonSource input) throws IOException { + public ProtobufMecanumDriveWheelAccelerations mergeFrom(final JsonSource input) throws + IOException { if (!input.beginObject()) { return this; } @@ -3391,8 +4252,8 @@ public ProtobufMecanumDriveWheelSpeeds mergeFrom(final JsonSource input) throws } @Override - public ProtobufMecanumDriveWheelSpeeds clone() { - return new ProtobufMecanumDriveWheelSpeeds().copyFrom(this); + public ProtobufMecanumDriveWheelAccelerations clone() { + return new ProtobufMecanumDriveWheelAccelerations().copyFrom(this); } @Override @@ -3400,41 +4261,41 @@ public boolean isEmpty() { return ((bitField0_) == 0); } - public static ProtobufMecanumDriveWheelSpeeds parseFrom(final byte[] data) throws + public static ProtobufMecanumDriveWheelAccelerations parseFrom(final byte[] data) throws InvalidProtocolBufferException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), data).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelAccelerations(), data).checkInitialized(); } - public static ProtobufMecanumDriveWheelSpeeds parseFrom(final ProtoSource input) throws + public static ProtobufMecanumDriveWheelAccelerations parseFrom(final ProtoSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), input).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelAccelerations(), input).checkInitialized(); } - public static ProtobufMecanumDriveWheelSpeeds parseFrom(final JsonSource input) throws + public static ProtobufMecanumDriveWheelAccelerations parseFrom(final JsonSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelSpeeds(), input).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelAccelerations(), input).checkInitialized(); } /** - * @return factory for creating ProtobufMecanumDriveWheelSpeeds messages + * @return factory for creating ProtobufMecanumDriveWheelAccelerations messages */ - public static MessageFactory getFactory() { - return ProtobufMecanumDriveWheelSpeedsFactory.INSTANCE; + public static MessageFactory getFactory() { + return ProtobufMecanumDriveWheelAccelerationsFactory.INSTANCE; } /** * @return this type's descriptor. */ public static Descriptors.Descriptor getDescriptor() { - return Kinematics.wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor; + return Kinematics.wpi_proto_ProtobufMecanumDriveWheelAccelerations_descriptor; } - private enum ProtobufMecanumDriveWheelSpeedsFactory implements MessageFactory { + private enum ProtobufMecanumDriveWheelAccelerationsFactory implements MessageFactory { INSTANCE; @Override - public ProtobufMecanumDriveWheelSpeeds create() { - return ProtobufMecanumDriveWheelSpeeds.newInstance(); + public ProtobufMecanumDriveWheelAccelerations create() { + return ProtobufMecanumDriveWheelAccelerations.newInstance(); } } @@ -4447,4 +5308,341 @@ static class FieldNames { static final FieldName angle = FieldName.forField("angle"); } } + + /** + * Protobuf type {@code ProtobufSwerveModuleAccelerations} + */ + public static final class ProtobufSwerveModuleAccelerations extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double acceleration = 1; + */ + private double acceleration; + + /** + * optional double angular_acceleration = 2; + */ + private double angularAcceleration; + + private ProtobufSwerveModuleAccelerations() { + } + + /** + * @return a new empty instance of {@code ProtobufSwerveModuleAccelerations} + */ + public static ProtobufSwerveModuleAccelerations newInstance() { + return new ProtobufSwerveModuleAccelerations(); + } + + /** + * optional double acceleration = 1; + * @return whether the acceleration field is set + */ + public boolean hasAcceleration() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double acceleration = 1; + * @return this + */ + public ProtobufSwerveModuleAccelerations clearAcceleration() { + bitField0_ &= ~0x00000001; + acceleration = 0D; + return this; + } + + /** + * optional double acceleration = 1; + * @return the acceleration + */ + public double getAcceleration() { + return acceleration; + } + + /** + * optional double acceleration = 1; + * @param value the acceleration to set + * @return this + */ + public ProtobufSwerveModuleAccelerations setAcceleration(final double value) { + bitField0_ |= 0x00000001; + acceleration = value; + return this; + } + + /** + * optional double angular_acceleration = 2; + * @return whether the angularAcceleration field is set + */ + public boolean hasAngularAcceleration() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double angular_acceleration = 2; + * @return this + */ + public ProtobufSwerveModuleAccelerations clearAngularAcceleration() { + bitField0_ &= ~0x00000002; + angularAcceleration = 0D; + return this; + } + + /** + * optional double angular_acceleration = 2; + * @return the angularAcceleration + */ + public double getAngularAcceleration() { + return angularAcceleration; + } + + /** + * optional double angular_acceleration = 2; + * @param value the angularAcceleration to set + * @return this + */ + public ProtobufSwerveModuleAccelerations setAngularAcceleration(final double value) { + bitField0_ |= 0x00000002; + angularAcceleration = value; + return this; + } + + @Override + public ProtobufSwerveModuleAccelerations copyFrom( + final ProtobufSwerveModuleAccelerations other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + acceleration = other.acceleration; + angularAcceleration = other.angularAcceleration; + } + return this; + } + + @Override + public ProtobufSwerveModuleAccelerations mergeFrom( + final ProtobufSwerveModuleAccelerations other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasAcceleration()) { + setAcceleration(other.acceleration); + } + if (other.hasAngularAcceleration()) { + setAngularAcceleration(other.angularAcceleration); + } + return this; + } + + @Override + public ProtobufSwerveModuleAccelerations clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + acceleration = 0D; + angularAcceleration = 0D; + return this; + } + + @Override + public ProtobufSwerveModuleAccelerations clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufSwerveModuleAccelerations)) { + return false; + } + ProtobufSwerveModuleAccelerations other = (ProtobufSwerveModuleAccelerations) o; + return bitField0_ == other.bitField0_ + && (!hasAcceleration() || ProtoUtil.isEqual(acceleration, other.acceleration)) + && (!hasAngularAcceleration() || ProtoUtil.isEqual(angularAcceleration, other.angularAcceleration)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 9); + output.writeDoubleNoTag(acceleration); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(angularAcceleration); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += 9; + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufSwerveModuleAccelerations mergeFrom(final ProtoSource input) throws IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 9: { + // acceleration + acceleration = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 17) { + break; + } + } + case 17: { + // angularAcceleration + angularAcceleration = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.acceleration, acceleration); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.angularAcceleration, angularAcceleration); + } + output.endObject(); + } + + @Override + public ProtobufSwerveModuleAccelerations mergeFrom(final JsonSource input) throws IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case -267299712: { + if (input.isAtField(FieldNames.acceleration)) { + if (!input.trySkipNullValue()) { + acceleration = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case -1643072638: + case 1425936317: { + if (input.isAtField(FieldNames.angularAcceleration)) { + if (!input.trySkipNullValue()) { + angularAcceleration = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufSwerveModuleAccelerations clone() { + return new ProtobufSwerveModuleAccelerations().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufSwerveModuleAccelerations parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAccelerations(), data).checkInitialized(); + } + + public static ProtobufSwerveModuleAccelerations parseFrom(final ProtoSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAccelerations(), input).checkInitialized(); + } + + public static ProtobufSwerveModuleAccelerations parseFrom(final JsonSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAccelerations(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufSwerveModuleAccelerations messages + */ + public static MessageFactory getFactory() { + return ProtobufSwerveModuleAccelerationsFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Kinematics.wpi_proto_ProtobufSwerveModuleAccelerations_descriptor; + } + + private enum ProtobufSwerveModuleAccelerationsFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufSwerveModuleAccelerations create() { + return ProtobufSwerveModuleAccelerations.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName acceleration = FieldName.forField("acceleration"); + + static final FieldName angularAcceleration = FieldName.forField("angularAcceleration", "angular_acceleration"); + } + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java new file mode 100644 index 00000000000..e221d3c5d6d --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java @@ -0,0 +1,125 @@ +// 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; + +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; + +import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelAccelerationsProto; +import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelAccelerationsStruct; +import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.StructSerializable; + +/** Represents the wheel accelerations for a differential drive drivetrain. */ +public class DifferentialDriveWheelAccelerations implements ProtobufSerializable, StructSerializable { + /** Acceleration of the left side of the robot in meters per second squared. */ + public double left; + + /** Acceleration of the right side of the robot in meters per second squared. */ + public double right; + + /** DifferentialDriveWheelAccelerations protobuf for serialization. */ + public static final DifferentialDriveWheelAccelerationsProto proto = + new DifferentialDriveWheelAccelerationsProto(); + + /** DifferentialDriveWheelAccelerations struct for serialization. */ + public static final DifferentialDriveWheelAccelerationsStruct struct = + new DifferentialDriveWheelAccelerationsStruct(); + + /** Constructs a DifferentialDriveWheelAccelerations with zeros for left and right accelerations. */ + public DifferentialDriveWheelAccelerations() {} + + /** + * Constructs a DifferentialDriveWheelAccelerations. + * + * @param left The left acceleration in meters per second squared. + * @param right The right acceleration in meters per second squared. + */ + public DifferentialDriveWheelAccelerations(double left, double right) { + this.left = left; + this.right = right; + } + + /** + * Constructs a DifferentialDriveWheelAccelerations. + * + * @param left The left acceleration in meters per second squared. + * @param right The right acceleration in meters per second squared. + */ + public DifferentialDriveWheelAccelerations(LinearAcceleration left, LinearAcceleration right) { + this(left.in(MetersPerSecondPerSecond), right.in(MetersPerSecondPerSecond)); + } + + /** + * Adds two DifferentialDriveWheelAccelerations and returns the sum. + * + *

For example, DifferentialDriveWheelAccelerations{1.0, 0.5} + DifferentialDriveWheelAccelerations{2.0, 1.5} + * = DifferentialDriveWheelAccelerations{3.0, 2.0} + * + * @param other The DifferentialDriveWheelAccelerations to add. + * @return The sum of the DifferentialDriveWheelAccelerations. + */ + public DifferentialDriveWheelAccelerations plus(DifferentialDriveWheelAccelerations other) { + return new DifferentialDriveWheelAccelerations(left + other.left, right + other.right); + } + + /** + * Subtracts the other DifferentialDriveWheelAccelerations from the current DifferentialDriveWheelAccelerations + * and returns the difference. + * + *

For example, DifferentialDriveWheelAccelerations{5.0, 4.0} - DifferentialDriveWheelAccelerations{1.0, 2.0} + * = DifferentialDriveWheelAccelerations{4.0, 2.0} + * + * @param other The DifferentialDriveWheelAccelerations to subtract. + * @return The difference between the two DifferentialDriveWheelAccelerations. + */ + public DifferentialDriveWheelAccelerations minus(DifferentialDriveWheelAccelerations other) { + return new DifferentialDriveWheelAccelerations(left - other.left, right - other.right); + } + + /** + * Returns the inverse of the current DifferentialDriveWheelAccelerations. This is equivalent to negating + * all components of the DifferentialDriveWheelAccelerations. + * + * @return The inverse of the current DifferentialDriveWheelAccelerations. + */ + public DifferentialDriveWheelAccelerations unaryMinus() { + return new DifferentialDriveWheelAccelerations(-left, -right); + } + + /** + * Multiplies the DifferentialDriveWheelAccelerations by a scalar and returns the new + * DifferentialDriveWheelAccelerations. + * + *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2 = DifferentialDriveWheelAccelerations{4.0, + * 5.0} + * + * @param scalar The scalar to multiply by. + * @return The scaled DifferentialDriveWheelAccelerations. + */ + public DifferentialDriveWheelAccelerations times(double scalar) { + return new DifferentialDriveWheelAccelerations(left * scalar, right * scalar); + } + + /** + * Divides the DifferentialDriveWheelAccelerations by a scalar and returns the new + * DifferentialDriveWheelAccelerations. + * + *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2 = DifferentialDriveWheelAccelerations{1.0, + * 1.25} + * + * @param scalar The scalar to divide by. + * @return The scaled DifferentialDriveWheelAccelerations. + */ + public DifferentialDriveWheelAccelerations div(double scalar) { + return new DifferentialDriveWheelAccelerations(left / scalar, right / scalar); + } + + @Override + public String toString() { + return String.format( + "DifferentialDriveWheelAccelerations(Left: %.2f m/s², Right: %.2f m/s²)", left, right); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java new file mode 100644 index 00000000000..02da767f434 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java @@ -0,0 +1,154 @@ +// 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; + +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; + +import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelAccelerationsProto; +import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelAccelerationsStruct; +import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.StructSerializable; + +/** Represents the wheel accelerations for a mecanum drive drivetrain. */ +public class MecanumDriveWheelAccelerations implements ProtobufSerializable, StructSerializable { + /** Acceleration of the front left wheel in meters per second squared. */ + public double frontLeft; + + /** Acceleration of the front right wheel in meters per second squared. */ + public double frontRight; + + /** Acceleration of the rear left wheel in meters per second squared. */ + public double rearLeft; + + /** Acceleration of the rear right wheel in meters per second squared. */ + public double rearRight; + + /** MecanumDriveWheelAccelerations protobuf for serialization. */ + public static final MecanumDriveWheelAccelerationsProto proto = new MecanumDriveWheelAccelerationsProto(); + + /** MecanumDriveWheelAccelerations struct for serialization. */ + public static final MecanumDriveWheelAccelerationsStruct struct = new MecanumDriveWheelAccelerationsStruct(); + + /** Constructs a MecanumDriveWheelAccelerations with zeros for all member fields. */ + public MecanumDriveWheelAccelerations() {} + + /** + * Constructs a MecanumDriveWheelAccelerations. + * + * @param frontLeft Acceleration of the front left wheel in meters per second squared. + * @param frontRight Acceleration of the front right wheel in meters per second squared. + * @param rearLeft Acceleration of the rear left wheel in meters per second squared. + * @param rearRight Acceleration of the rear right wheel in meters per second squared. + */ + public MecanumDriveWheelAccelerations( + double frontLeft, double frontRight, double rearLeft, double rearRight) { + this.frontLeft = frontLeft; + this.frontRight = frontRight; + this.rearLeft = rearLeft; + this.rearRight = rearRight; + } + + /** + * Constructs a MecanumDriveWheelAccelerations. + * + * @param frontLeft Acceleration of the front left wheel in meters per second squared. + * @param frontRight Acceleration of the front right wheel in meters per second squared. + * @param rearLeft Acceleration of the rear left wheel in meters per second squared. + * @param rearRight Acceleration of the rear right wheel in meters per second squared. + */ + public MecanumDriveWheelAccelerations( + LinearAcceleration frontLeft, + LinearAcceleration frontRight, + LinearAcceleration rearLeft, + LinearAcceleration rearRight) { + this( + frontLeft.in(MetersPerSecondPerSecond), + frontRight.in(MetersPerSecondPerSecond), + rearLeft.in(MetersPerSecondPerSecond), + rearRight.in(MetersPerSecondPerSecond)); + } + + /** + * Adds two MecanumDriveWheelAccelerations and returns the sum. + * + *

For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} + MecanumDriveWheelAccelerations{2.0, 1.5, + * 0.5, 1.0} = MecanumDriveWheelAccelerations{3.0, 2.0, 2.5, 2.5} + * + * @param other The MecanumDriveWheelAccelerations to add. + * @return The sum of the MecanumDriveWheelAccelerations. + */ + public MecanumDriveWheelAccelerations plus(MecanumDriveWheelAccelerations other) { + return new MecanumDriveWheelAccelerations( + frontLeft + other.frontLeft, + frontRight + other.frontRight, + rearLeft + other.rearLeft, + rearRight + other.rearRight); + } + + /** + * Subtracts the other MecanumDriveWheelAccelerations from the current MecanumDriveWheelAccelerations and + * returns the difference. + * + *

For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} - MecanumDriveWheelAccelerations{1.0, 2.0, + * 3.0, 0.5} = MecanumDriveWheelAccelerations{4.0, 2.0, 3.0, 2.0} + * + * @param other The MecanumDriveWheelAccelerations to subtract. + * @return The difference between the two MecanumDriveWheelAccelerations. + */ + public MecanumDriveWheelAccelerations minus(MecanumDriveWheelAccelerations other) { + return new MecanumDriveWheelAccelerations( + frontLeft - other.frontLeft, + frontRight - other.frontRight, + rearLeft - other.rearLeft, + rearRight - other.rearRight); + } + + /** + * Returns the inverse of the current MecanumDriveWheelAccelerations. This is equivalent to negating all + * components of the MecanumDriveWheelAccelerations. + * + * @return The inverse of the current MecanumDriveWheelAccelerations. + */ + public MecanumDriveWheelAccelerations unaryMinus() { + return new MecanumDriveWheelAccelerations(-frontLeft, -frontRight, -rearLeft, -rearRight); + } + + /** + * Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the new MecanumDriveWheelAccelerations. + * + *

For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 = MecanumDriveWheelAccelerations{4.0, + * 5.0, 6.0, 7.0} + * + * @param scalar The scalar to multiply by. + * @return The scaled MecanumDriveWheelAccelerations. + */ + public MecanumDriveWheelAccelerations times(double scalar) { + return new MecanumDriveWheelAccelerations( + frontLeft * scalar, frontRight * scalar, rearLeft * scalar, rearRight * scalar); + } + + /** + * Divides the MecanumDriveWheelAccelerations by a scalar and returns the new MecanumDriveWheelAccelerations. + * + *

For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 = MecanumDriveWheelAccelerations{1.0, + * 1.25, 0.75, 0.5} + * + * @param scalar The scalar to divide by. + * @return The scaled MecanumDriveWheelAccelerations. + */ + public MecanumDriveWheelAccelerations div(double scalar) { + return new MecanumDriveWheelAccelerations( + frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar); + } + + @Override + public String toString() { + return String.format( + "MecanumDriveWheelAccelerations(Front Left: %.2f m/s², Front Right: %.2f m/s², " + + "Rear Left: %.2f m/s², Rear Right: %.2f m/s²)", + frontLeft, frontRight, rearLeft, rearRight); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java new file mode 100644 index 00000000000..6d292a8cfae --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java @@ -0,0 +1,145 @@ +// 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; + +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; + +import edu.wpi.first.math.kinematics.proto.SwerveModuleAccelerationsProto; +import edu.wpi.first.math.kinematics.struct.SwerveModuleAccelerationsStruct; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.StructSerializable; +import java.util.Objects; + +/** Represents the accelerations of one swerve module. */ +public class SwerveModuleAccelerations + implements Comparable, ProtobufSerializable, StructSerializable { + /** Acceleration of the wheel of the module in meters per second squared. */ + public double acceleration; + + /** Angular acceleration of the module in radians per second squared. */ + public double angularAcceleration; + + /** SwerveModuleAccelerations protobuf for serialization. */ + public static final SwerveModuleAccelerationsProto proto = new SwerveModuleAccelerationsProto(); + + /** SwerveModuleAccelerations struct for serialization. */ + public static final SwerveModuleAccelerationsStruct struct = new SwerveModuleAccelerationsStruct(); + + /** Constructs a SwerveModuleAccelerations with zeros for acceleration and angular acceleration. */ + public SwerveModuleAccelerations() {} + + /** + * Constructs a SwerveModuleAccelerations. + * + * @param acceleration The acceleration of the wheel of the module in meters per second squared. + * @param angularAcceleration The angular acceleration of the module in radians per second squared. + */ + public SwerveModuleAccelerations(double acceleration, double angularAcceleration) { + this.acceleration = acceleration; + this.angularAcceleration = angularAcceleration; + } + + /** + * Constructs a SwerveModuleAccelerations. + * + * @param acceleration The acceleration of the wheel of the module. + * @param angularAcceleration The angular acceleration of the module. + */ + public SwerveModuleAccelerations( + LinearAcceleration acceleration, AngularAcceleration angularAcceleration) { + this( + acceleration.in(MetersPerSecondPerSecond), + angularAcceleration.in(RadiansPerSecondPerSecond)); + } + + /** + * Adds two SwerveModuleAccelerations and returns the sum. + * + * @param other The SwerveModuleAccelerations to add. + * @return The sum of the SwerveModuleAccelerations. + */ + public SwerveModuleAccelerations plus(SwerveModuleAccelerations other) { + return new SwerveModuleAccelerations( + acceleration + other.acceleration, angularAcceleration + other.angularAcceleration); + } + + /** + * Subtracts the other SwerveModuleAccelerations from the current SwerveModuleAccelerations and + * returns the difference. + * + * @param other The SwerveModuleAccelerations to subtract. + * @return The difference between the two SwerveModuleAccelerations. + */ + public SwerveModuleAccelerations minus(SwerveModuleAccelerations other) { + return new SwerveModuleAccelerations( + acceleration - other.acceleration, angularAcceleration - other.angularAcceleration); + } + + /** + * Returns the inverse of the current SwerveModuleAccelerations. This is equivalent to negating + * all components of the SwerveModuleAccelerations. + * + * @return The inverse of the current SwerveModuleAccelerations. + */ + public SwerveModuleAccelerations unaryMinus() { + return new SwerveModuleAccelerations(-acceleration, -angularAcceleration); + } + + /** + * Multiplies the SwerveModuleAccelerations by a scalar and returns the new + * SwerveModuleAccelerations. + * + * @param scalar The scalar to multiply by. + * @return The scaled SwerveModuleAccelerations. + */ + public SwerveModuleAccelerations times(double scalar) { + return new SwerveModuleAccelerations(acceleration * scalar, angularAcceleration * scalar); + } + + /** + * Divides the SwerveModuleAccelerations by a scalar and returns the new + * SwerveModuleAccelerations. + * + * @param scalar The scalar to divide by. + * @return The scaled SwerveModuleAccelerations. + */ + public SwerveModuleAccelerations div(double scalar) { + return new SwerveModuleAccelerations(acceleration / scalar, angularAcceleration / scalar); + } + + @Override + public boolean equals(Object obj) { + return obj instanceof SwerveModuleAccelerations other + && Math.abs(other.acceleration - acceleration) < 1E-9 + && Math.abs(other.angularAcceleration - angularAcceleration) < 1E-9; + } + + @Override + public int hashCode() { + return Objects.hash(acceleration, angularAcceleration); + } + + /** + * Compares two swerve module accelerations. One swerve module is "greater" than the other if its + * acceleration is higher than the other. + * + * @param other The other swerve module. + * @return 1 if this is greater, 0 if both are equal, -1 if other is greater. + */ + @Override + public int compareTo(SwerveModuleAccelerations other) { + return Double.compare(this.acceleration, other.acceleration); + } + + @Override + public String toString() { + return String.format( + "SwerveModuleAccelerations(Acceleration: %.2f m/s², Angular Acceleration: %.2f rad/s²)", + acceleration, angularAcceleration); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java new file mode 100644 index 00000000000..dc0c91ac961 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelAccelerations; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelAccelerations; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveWheelAccelerationsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelAccelerations.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveWheelAccelerations.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveWheelAccelerations createMessage() { + return ProtobufDifferentialDriveWheelAccelerations.newInstance(); + } + + @Override + public DifferentialDriveWheelAccelerations unpack(ProtobufDifferentialDriveWheelAccelerations msg) { + return new DifferentialDriveWheelAccelerations(msg.getLeft(), msg.getRight()); + } + + @Override + public void pack(ProtobufDifferentialDriveWheelAccelerations msg, DifferentialDriveWheelAccelerations value) { + msg.setLeft(value.left); + msg.setRight(value.right); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java new file mode 100644 index 00000000000..adb1cc89839 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelAccelerations; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelAccelerations; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class MecanumDriveWheelAccelerationsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MecanumDriveWheelAccelerations.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMecanumDriveWheelAccelerations.getDescriptor(); + } + + @Override + public ProtobufMecanumDriveWheelAccelerations createMessage() { + return ProtobufMecanumDriveWheelAccelerations.newInstance(); + } + + @Override + public MecanumDriveWheelAccelerations unpack(ProtobufMecanumDriveWheelAccelerations msg) { + return new MecanumDriveWheelAccelerations( + msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight()); + } + + @Override + public void pack(ProtobufMecanumDriveWheelAccelerations msg, MecanumDriveWheelAccelerations value) { + msg.setFrontLeft(value.frontLeft); + msg.setFrontRight(value.frontRight); + msg.setRearLeft(value.rearLeft); + msg.setRearRight(value.rearRight); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java new file mode 100644 index 00000000000..13adba8a962 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.SwerveModuleAccelerations; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModuleAccelerations; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class SwerveModuleAccelerationsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return SwerveModuleAccelerations.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufSwerveModuleAccelerations.getDescriptor(); + } + + @Override + public ProtobufSwerveModuleAccelerations createMessage() { + return ProtobufSwerveModuleAccelerations.newInstance(); + } + + @Override + public SwerveModuleAccelerations unpack(ProtobufSwerveModuleAccelerations msg) { + return new SwerveModuleAccelerations(msg.getAcceleration(), msg.getAngularAcceleration()); + } + + @Override + public void pack(ProtobufSwerveModuleAccelerations msg, SwerveModuleAccelerations value) { + msg.setAcceleration(value.acceleration); + msg.setAngularAcceleration(value.angularAcceleration); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java new file mode 100644 index 00000000000..8dde9cd18e5 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java @@ -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. + +package edu.wpi.first.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelAccelerations; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DifferentialDriveWheelAccelerationsStruct implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelAccelerations.class; + } + + @Override + public String getTypeName() { + return "DifferentialDriveWheelAccelerations"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double left;double right"; + } + + @Override + public DifferentialDriveWheelAccelerations unpack(ByteBuffer bb) { + double left = bb.getDouble(); + double right = bb.getDouble(); + return new DifferentialDriveWheelAccelerations(left, right); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveWheelAccelerations value) { + bb.putDouble(value.left); + bb.putDouble(value.right); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java new file mode 100644 index 00000000000..f84f8b12749 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelAccelerations; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class MecanumDriveWheelAccelerationsStruct implements Struct { + @Override + public Class getTypeClass() { + return MecanumDriveWheelAccelerations.class; + } + + @Override + public String getTypeName() { + return "MecanumDriveWheelAccelerations"; + } + + @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 MecanumDriveWheelAccelerations unpack(ByteBuffer bb) { + double frontLeft = bb.getDouble(); + double frontRight = bb.getDouble(); + double rearLeft = bb.getDouble(); + double rearRight = bb.getDouble(); + return new MecanumDriveWheelAccelerations(frontLeft, frontRight, rearLeft, rearRight); + } + + @Override + public void pack(ByteBuffer bb, MecanumDriveWheelAccelerations value) { + bb.putDouble(value.frontLeft); + bb.putDouble(value.frontRight); + bb.putDouble(value.rearLeft); + bb.putDouble(value.rearRight); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java new file mode 100644 index 00000000000..cd8752459ab --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java @@ -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. + +package edu.wpi.first.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.SwerveModuleAccelerations; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class SwerveModuleAccelerationsStruct implements Struct { + @Override + public Class getTypeClass() { + return SwerveModuleAccelerations.class; + } + + @Override + public String getTypeName() { + return "SwerveModuleAccelerations"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double acceleration;double angular_acceleration"; + } + + @Override + public SwerveModuleAccelerations unpack(ByteBuffer bb) { + double acceleration = bb.getDouble(); + double angularAcceleration = bb.getDouble(); + return new SwerveModuleAccelerations(acceleration, angularAcceleration); + } + + @Override + public void pack(ByteBuffer bb, SwerveModuleAccelerations value) { + bb.putDouble(value.acceleration); + bb.putDouble(value.angularAcceleration); + } +} diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto index c705575898f..68fe600dd19 100644 --- a/wpimath/src/main/proto/kinematics.proto +++ b/wpimath/src/main/proto/kinematics.proto @@ -27,6 +27,11 @@ message ProtobufDifferentialDriveWheelSpeeds { double right = 2; } +message ProtobufDifferentialDriveWheelAccelerations { + double left = 1; + double right = 2; +} + message ProtobufDifferentialDriveWheelPositions { double left = 1; double right = 2; @@ -53,6 +58,13 @@ message ProtobufMecanumDriveWheelSpeeds { double rear_right = 4; } +message ProtobufMecanumDriveWheelAccelerations { + double front_left = 1; + double front_right = 2; + double rear_left = 3; + double rear_right = 4; +} + message ProtobufSwerveDriveKinematics { repeated ProtobufTranslation2d modules = 1; } @@ -66,3 +78,8 @@ message ProtobufSwerveModuleState { double speed = 1; ProtobufRotation2d angle = 2; } + +message ProtobufSwerveModuleAccelerations { + double acceleration = 1; + double angular_acceleration = 2; +} From 0f6b1e0df7f2ef5a15571465fe5a6a2a3372f5a8 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 17:10:07 -0400 Subject: [PATCH 08/82] feat: c++ implementations of DifferentialDriveWheelAccelerations, MecanumDriveWheelAccelerations, and SwerveModuleAccelerations with relevant protobufs/structs Signed-off-by: Zach Harel --- ...fferentialDriveWheelAccelerationsProto.cpp | 29 ++++ .../MecanumDriveWheelAccelerationsProto.cpp | 33 +++++ .../proto/SwerveModuleAccelerationsProto.cpp | 29 ++++ ...ferentialDriveWheelAccelerationsStruct.cpp | 29 ++++ .../MecanumDriveWheelAccelerationsStruct.cpp | 39 ++++++ .../SwerveModuleAccelerationsStruct.cpp | 29 ++++ .../DifferentialDriveWheelAccelerations.h | 113 ++++++++++++++++ .../MecanumDriveWheelAccelerations.h | 125 ++++++++++++++++++ .../kinematics/SwerveModuleAccelerations.h | 98 ++++++++++++++ ...DifferentialDriveWheelAccelerationsProto.h | 20 +++ .../MecanumDriveWheelAccelerationsProto.h | 20 +++ .../proto/SwerveModuleAccelerationsProto.h | 20 +++ ...ifferentialDriveWheelAccelerationsStruct.h | 24 ++++ .../MecanumDriveWheelAccelerationsStruct.h | 24 ++++ .../struct/SwerveModuleAccelerationsStruct.h | 24 ++++ 15 files changed, 656 insertions(+) create mode 100644 wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp create mode 100644 wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelAccelerations.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelAccelerations.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleAccelerationsProto.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp new file mode 100644 index 00000000000..72e61d9b610 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h" + +#include "wpimath/protobuf/kinematics.npb.h" + +std::optional wpi::Protobuf::Unpack( + InputStream& stream) { + wpi_proto_ProtobufDifferentialDriveWheelAccelerations msg; + if (!stream.Decode(msg)) { + return {}; + } + + return frc::DifferentialDriveWheelAccelerations{ + units::meters_per_second_squared_t{msg.left}, + units::meters_per_second_squared_t{msg.right}, + }; +} + +bool wpi::Protobuf::Pack(OutputStream& stream, + const frc::DifferentialDriveWheelAccelerations& value) { + wpi_proto_ProtobufDifferentialDriveWheelAccelerations msg{ + .left = value.left.value(), + .right = value.right.value(), + }; + return stream.Encode(msg); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp new file mode 100644 index 00000000000..77a5149aa3b --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h" + +#include "wpimath/protobuf/kinematics.npb.h" + +std::optional wpi::Protobuf::Unpack( + InputStream& stream) { + wpi_proto_ProtobufMecanumDriveWheelAccelerations msg; + if (!stream.Decode(msg)) { + return {}; + } + + return frc::MecanumDriveWheelAccelerations{ + units::meters_per_second_squared_t{msg.front_left}, + units::meters_per_second_squared_t{msg.front_right}, + units::meters_per_second_squared_t{msg.rear_left}, + units::meters_per_second_squared_t{msg.rear_right}, + }; +} + +bool wpi::Protobuf::Pack(OutputStream& stream, + const frc::MecanumDriveWheelAccelerations& value) { + wpi_proto_ProtobufMecanumDriveWheelAccelerations msg{ + .front_left = value.frontLeft.value(), + .front_right = value.frontRight.value(), + .rear_left = value.rearLeft.value(), + .rear_right = value.rearRight.value(), + }; + return stream.Encode(msg); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp new file mode 100644 index 00000000000..9a9808fae8d --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/proto/SwerveModuleAccelerationsProto.h" + +#include "wpimath/protobuf/kinematics.npb.h" + +std::optional wpi::Protobuf::Unpack( + InputStream& stream) { + wpi_proto_ProtobufSwerveModuleAccelerations msg; + if (!stream.Decode(msg)) { + return {}; + } + + return frc::SwerveModuleAccelerations{ + units::meters_per_second_squared_t{msg.acceleration}, + units::radians_per_second_squared_t{msg.angular_acceleration}, + }; +} + +bool wpi::Protobuf::Pack(OutputStream& stream, + const frc::SwerveModuleAccelerations& value) { + wpi_proto_ProtobufSwerveModuleAccelerations msg{ + .acceleration = value.acceleration.value(), + .angular_acceleration = value.angularAcceleration.value(), + }; + return stream.Encode(msg); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp new file mode 100644 index 00000000000..a21ea36a1d2 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h" + +#include + +#include "frc/kinematics/DifferentialDriveWheelAccelerations.h" + +frc::DifferentialDriveWheelAccelerations wpi::Struct::Unpack( + std::span data) { + constexpr size_t kLeftOff = 0; + constexpr size_t kRightOff = kLeftOff + 8; + return frc::DifferentialDriveWheelAccelerations{ + units::meters_per_second_squared_t{ + wpi::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::UnpackStruct(data)}, + }; +} + +void wpi::Struct::Pack( + std::span data, const frc::DifferentialDriveWheelAccelerations& value) { + constexpr size_t kLeftOff = 0; + constexpr size_t kRightOff = kLeftOff + 8; + wpi::PackStruct(data, value.left.value()); + wpi::PackStruct(data, value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp new file mode 100644 index 00000000000..164eea67919 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp @@ -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. + +#include "frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h" + +#include + +#include "frc/kinematics/MecanumDriveWheelAccelerations.h" + +frc::MecanumDriveWheelAccelerations wpi::Struct::Unpack( + std::span data) { + constexpr size_t kFrontLeftOff = 0; + constexpr size_t kFrontRightOff = kFrontLeftOff + 8; + constexpr size_t kRearLeftOff = kFrontRightOff + 8; + constexpr size_t kRearRightOff = kRearLeftOff + 8; + return frc::MecanumDriveWheelAccelerations{ + units::meters_per_second_squared_t{ + wpi::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::UnpackStruct(data)}, + }; +} + +void wpi::Struct::Pack( + std::span data, const frc::MecanumDriveWheelAccelerations& value) { + constexpr size_t kFrontLeftOff = 0; + constexpr size_t kFrontRightOff = kFrontLeftOff + 8; + constexpr size_t kRearLeftOff = kFrontRightOff + 8; + constexpr size_t kRearRightOff = kRearLeftOff + 8; + wpi::PackStruct(data, value.frontLeft.value()); + wpi::PackStruct(data, value.frontRight.value()); + wpi::PackStruct(data, value.rearLeft.value()); + wpi::PackStruct(data, value.rearRight.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp new file mode 100644 index 00000000000..94437430122 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/struct/SwerveModuleAccelerationsStruct.h" + +#include + +#include "frc/kinematics/SwerveModuleAccelerations.h" + +frc::SwerveModuleAccelerations wpi::Struct::Unpack( + std::span data) { + constexpr size_t kAccelerationOff = 0; + constexpr size_t kAngularAccelerationOff = kAccelerationOff + 8; + return frc::SwerveModuleAccelerations{ + units::meters_per_second_squared_t{ + wpi::UnpackStruct(data)}, + units::radians_per_second_squared_t{ + wpi::UnpackStruct(data)}, + }; +} + +void wpi::Struct::Pack( + std::span data, const frc::SwerveModuleAccelerations& value) { + constexpr size_t kAccelerationOff = 0; + constexpr size_t kAngularAccelerationOff = kAccelerationOff + 8; + wpi::PackStruct(data, value.acceleration.value()); + wpi::PackStruct(data, value.angularAcceleration.value()); +} diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelAccelerations.h new file mode 100644 index 00000000000..2446bfb9557 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelAccelerations.h @@ -0,0 +1,113 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "units/acceleration.h" +#include "units/math.h" + +namespace frc { +/** + * Represents the wheel accelerations for a differential drive drivetrain. + */ +struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations { + /** + * Acceleration of the left side of the robot. + */ + units::meters_per_second_squared_t left = 0_mps_sq; + + /** + * Acceleration of the right side of the robot. + */ + units::meters_per_second_squared_t right = 0_mps_sq; + + /** + * Adds two DifferentialDriveWheelAccelerations and returns the sum. + * + *

For example, DifferentialDriveWheelAccelerations{1.0, 0.5} + + * DifferentialDriveWheelAccelerations{2.0, 1.5} = + * DifferentialDriveWheelAccelerations{3.0, 2.0} + * + * @param other The DifferentialDriveWheelAccelerations to add. + * + * @return The sum of the DifferentialDriveWheelAccelerations. + */ + constexpr DifferentialDriveWheelAccelerations operator+( + const DifferentialDriveWheelAccelerations& other) const { + return {left + other.left, right + other.right}; + } + + /** + * Subtracts the other DifferentialDriveWheelAccelerations from the current + * DifferentialDriveWheelAccelerations and returns the difference. + * + *

For example, DifferentialDriveWheelAccelerations{5.0, 4.0} - + * DifferentialDriveWheelAccelerations{1.0, 2.0} = + * DifferentialDriveWheelAccelerations{4.0, 2.0} + * + * @param other The DifferentialDriveWheelAccelerations to subtract. + * + * @return The difference between the two DifferentialDriveWheelAccelerations. + */ + constexpr DifferentialDriveWheelAccelerations operator-( + const DifferentialDriveWheelAccelerations& other) const { + return *this + -other; + } + + /** + * Returns the inverse of the current DifferentialDriveWheelAccelerations. + * This is equivalent to negating all components of the + * DifferentialDriveWheelAccelerations. + * + * @return The inverse of the current DifferentialDriveWheelAccelerations. + */ + constexpr DifferentialDriveWheelAccelerations operator-() const { + return {-left, -right}; + } + + /** + * Multiplies the DifferentialDriveWheelAccelerations by a scalar and returns the new + * DifferentialDriveWheelAccelerations. + * + *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2 + * = DifferentialDriveWheelAccelerations{4.0, 5.0} + * + * @param scalar The scalar to multiply by. + * + * @return The scaled DifferentialDriveWheelAccelerations. + */ + constexpr DifferentialDriveWheelAccelerations operator*(double scalar) const { + return {scalar * left, scalar * right}; + } + + /** + * Divides the DifferentialDriveWheelAccelerations by a scalar and returns the new + * DifferentialDriveWheelAccelerations. + * + *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2 + * = DifferentialDriveWheelAccelerations{1.0, 1.25} + * + * @param scalar The scalar to divide by. + * + * @return The scaled DifferentialDriveWheelAccelerations. + */ + constexpr DifferentialDriveWheelAccelerations operator/(double scalar) const { + return operator*(1.0 / scalar); + } + + /** + * Compares two DifferentialDriveWheelAccelerations objects. + * + * @param other The other DifferentialDriveWheelAccelerations. + * + * @return True if the DifferentialDriveWheelAccelerations are equal. + */ + constexpr bool operator==(const DifferentialDriveWheelAccelerations& other) const = default; +}; +} // namespace frc + +#include "frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h" +#include "frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelAccelerations.h new file mode 100644 index 00000000000..c5e80ac95c5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelAccelerations.h @@ -0,0 +1,125 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include + +#include + +#include "units/acceleration.h" +#include "units/math.h" + +namespace frc { +/** + * Represents the wheel accelerations for a mecanum drive drivetrain. + */ +struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations { + /** + * Acceleration of the front-left wheel. + */ + units::meters_per_second_squared_t frontLeft = 0_mps_sq; + + /** + * Acceleration of the front-right wheel. + */ + units::meters_per_second_squared_t frontRight = 0_mps_sq; + + /** + * Acceleration of the rear-left wheel. + */ + units::meters_per_second_squared_t rearLeft = 0_mps_sq; + + /** + * Acceleration of the rear-right wheel. + */ + units::meters_per_second_squared_t rearRight = 0_mps_sq; + + /** + * Adds two MecanumDriveWheelAccelerations and returns the sum. + * + *

For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} + + * MecanumDriveWheelAccelerations{2.0, 1.5, 0.5, 1.0} = + * MecanumDriveWheelAccelerations{3.0, 2.0, 2.5, 2.5} + * + * @param other The MecanumDriveWheelAccelerations to add. + * @return The sum of the MecanumDriveWheelAccelerations. + */ + constexpr MecanumDriveWheelAccelerations operator+( + const MecanumDriveWheelAccelerations& other) const { + return {frontLeft + other.frontLeft, frontRight + other.frontRight, + rearLeft + other.rearLeft, rearRight + other.rearRight}; + } + + /** + * Subtracts the other MecanumDriveWheelAccelerations from the current + * MecanumDriveWheelAccelerations and returns the difference. + * + *

For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} - + * MecanumDriveWheelAccelerations{1.0, 2.0, 3.0, 0.5} = + * MecanumDriveWheelAccelerations{4.0, 2.0, 3.0, 2.0} + * + * @param other The MecanumDriveWheelAccelerations to subtract. + * @return The difference between the two MecanumDriveWheelAccelerations. + */ + constexpr MecanumDriveWheelAccelerations operator-( + const MecanumDriveWheelAccelerations& other) const { + return *this + -other; + } + + /** + * Returns the inverse of the current MecanumDriveWheelAccelerations. + * This is equivalent to negating all components of the + * MecanumDriveWheelAccelerations. + * + * @return The inverse of the current MecanumDriveWheelAccelerations. + */ + constexpr MecanumDriveWheelAccelerations operator-() const { + return {-frontLeft, -frontRight, -rearLeft, -rearRight}; + } + + /** + * Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the new + * MecanumDriveWheelAccelerations. + * + *

For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 = + * MecanumDriveWheelAccelerations{4.0, 5.0, 6.0, 7.0} + * + * @param scalar The scalar to multiply by. + * @return The scaled MecanumDriveWheelAccelerations. + */ + constexpr MecanumDriveWheelAccelerations operator*(double scalar) const { + return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft, + scalar * rearRight}; + } + + /** + * Divides the MecanumDriveWheelAccelerations by a scalar and returns the new + * MecanumDriveWheelAccelerations. + * + *

For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 = + * MecanumDriveWheelAccelerations{1.0, 1.25, 0.75, 0.5} + * + * @param scalar The scalar to divide by. + * @return The scaled MecanumDriveWheelAccelerations. + */ + constexpr MecanumDriveWheelAccelerations operator/(double scalar) const { + return operator*(1.0 / scalar); + } + + /** + * Compares two MecanumDriveWheelAccelerations objects. + * + * @param other The other MecanumDriveWheelAccelerations. + * + * @return True if the MecanumDriveWheelAccelerations are equal. + */ + constexpr bool operator==(const MecanumDriveWheelAccelerations& other) const = default; +}; +} // namespace frc + +#include "frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h" +#include "frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h new file mode 100644 index 00000000000..b02e353f4fd --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h @@ -0,0 +1,98 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "units/acceleration.h" +#include "units/angular_acceleration.h" +#include "units/math.h" + +namespace frc { +/** + * Represents the accelerations of one swerve module. + */ +struct WPILIB_DLLEXPORT SwerveModuleAccelerations { + /** + * Acceleration of the wheel of the module. + */ + units::meters_per_second_squared_t acceleration = 0_mps_sq; + + /** + * Angular acceleration of the module. + */ + units::radians_per_second_squared_t angularAcceleration = 0_rad_per_s_sq; + + /** + * Checks equality between this SwerveModuleAccelerations and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + constexpr bool operator==(const SwerveModuleAccelerations& other) const { + return units::math::abs(acceleration - other.acceleration) < 1E-9_mps_sq && + units::math::abs(angularAcceleration - other.angularAcceleration) < 1E-9_rad_per_s_sq; + } + + /** + * Adds two SwerveModuleAccelerations and returns the sum. + * + * @param other The SwerveModuleAccelerations to add. + * @return The sum of the SwerveModuleAccelerations. + */ + constexpr SwerveModuleAccelerations operator+( + const SwerveModuleAccelerations& other) const { + return {acceleration + other.acceleration, angularAcceleration + other.angularAcceleration}; + } + + /** + * Subtracts the other SwerveModuleAccelerations from the current + * SwerveModuleAccelerations and returns the difference. + * + * @param other The SwerveModuleAccelerations to subtract. + * @return The difference between the two SwerveModuleAccelerations. + */ + constexpr SwerveModuleAccelerations operator-( + const SwerveModuleAccelerations& other) const { + return *this + -other; + } + + /** + * Returns the inverse of the current SwerveModuleAccelerations. + * This is equivalent to negating all components of the + * SwerveModuleAccelerations. + * + * @return The inverse of the current SwerveModuleAccelerations. + */ + constexpr SwerveModuleAccelerations operator-() const { + return {-acceleration, -angularAcceleration}; + } + + /** + * Multiplies the SwerveModuleAccelerations by a scalar and returns the new + * SwerveModuleAccelerations. + * + * @param scalar The scalar to multiply by. + * @return The scaled SwerveModuleAccelerations. + */ + constexpr SwerveModuleAccelerations operator*(double scalar) const { + return {scalar * acceleration, scalar * angularAcceleration}; + } + + /** + * Divides the SwerveModuleAccelerations by a scalar and returns the new + * SwerveModuleAccelerations. + * + * @param scalar The scalar to divide by. + * @return The scaled SwerveModuleAccelerations. + */ + constexpr SwerveModuleAccelerations operator/(double scalar) const { + return operator*(1.0 / scalar); + } +}; +} // namespace frc + +#include "frc/kinematics/proto/SwerveModuleAccelerationsProto.h" +#include "frc/kinematics/struct/SwerveModuleAccelerationsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h new file mode 100644 index 00000000000..bf5a7590b02 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveWheelAccelerations.h" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelAccelerations; + using InputStream = wpi::ProtoInputStream; + using OutputStream = wpi::ProtoOutputStream; + static std::optional Unpack(InputStream& stream); + static bool Pack(OutputStream& stream, const frc::DifferentialDriveWheelAccelerations& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h new file mode 100644 index 00000000000..1b9a0206f26 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/MecanumDriveWheelAccelerations.h" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelAccelerations; + using InputStream = wpi::ProtoInputStream; + using OutputStream = wpi::ProtoOutputStream; + static std::optional Unpack(InputStream& stream); + static bool Pack(OutputStream& stream, const frc::MecanumDriveWheelAccelerations& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleAccelerationsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleAccelerationsProto.h new file mode 100644 index 00000000000..df02829ad46 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleAccelerationsProto.h @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/SwerveModuleAccelerations.h" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + using MessageStruct = wpi_proto_ProtobufSwerveModuleAccelerations; + using InputStream = wpi::ProtoInputStream; + using OutputStream = wpi::ProtoOutputStream; + static std::optional Unpack(InputStream& stream); + static bool Pack(OutputStream& stream, const frc::SwerveModuleAccelerations& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h new file mode 100644 index 00000000000..ff7483f27b9 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveWheelAccelerations.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeName() { return "DifferentialDriveWheelAccelerations"; } + static constexpr size_t GetSize() { return 16; } + static constexpr std::string_view GetSchema() { + return "double left;double right"; + } + + static frc::DifferentialDriveWheelAccelerations Unpack(std::span data); + static void Pack(std::span data, const frc::DifferentialDriveWheelAccelerations& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h new file mode 100644 index 00000000000..61ae0be871e --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/MecanumDriveWheelAccelerations.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeName() { return "MecanumDriveWheelAccelerations"; } + static constexpr size_t GetSize() { return 32; } + static constexpr std::string_view GetSchema() { + return "double front_left;double front_right;double rear_left;double rear_right"; + } + + static frc::MecanumDriveWheelAccelerations Unpack(std::span data); + static void Pack(std::span data, const frc::MecanumDriveWheelAccelerations& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h new file mode 100644 index 00000000000..70101d64638 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/SwerveModuleAccelerations.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeName() { return "SwerveModuleAccelerations"; } + static constexpr size_t GetSize() { return 16; } + static constexpr std::string_view GetSchema() { + return "double acceleration;double angular_acceleration"; + } + + static frc::SwerveModuleAccelerations Unpack(std::span data); + static void Pack(std::span data, const frc::SwerveModuleAccelerations& value); +}; + +static_assert(wpi::StructSerializable); From 84cf12d0eed8a18646a2391c622779bb1eaaed3f Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 17:38:06 -0400 Subject: [PATCH 09/82] fix: make spotless happy Signed-off-by: Zach Harel --- .../DifferentialDriveWheelAccelerations.java | 31 ++++++++------- .../MecanumDriveWheelAccelerations.java | 38 +++++++++++-------- .../kinematics/SwerveModuleAccelerations.java | 10 +++-- ...ferentialDriveWheelAccelerationsProto.java | 9 +++-- .../MecanumDriveWheelAccelerationsProto.java | 3 +- ...erentialDriveWheelAccelerationsStruct.java | 3 +- .../MecanumDriveWheelAccelerationsStruct.java | 3 +- 7 files changed, 58 insertions(+), 39 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java index e221d3c5d6d..a303ede5624 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java @@ -13,7 +13,8 @@ import edu.wpi.first.util.struct.StructSerializable; /** Represents the wheel accelerations for a differential drive drivetrain. */ -public class DifferentialDriveWheelAccelerations implements ProtobufSerializable, StructSerializable { +public class DifferentialDriveWheelAccelerations + implements ProtobufSerializable, StructSerializable { /** Acceleration of the left side of the robot in meters per second squared. */ public double left; @@ -28,7 +29,9 @@ public class DifferentialDriveWheelAccelerations implements ProtobufSerializable public static final DifferentialDriveWheelAccelerationsStruct struct = new DifferentialDriveWheelAccelerationsStruct(); - /** Constructs a DifferentialDriveWheelAccelerations with zeros for left and right accelerations. */ + /** + * Constructs a DifferentialDriveWheelAccelerations with zeros for left and right accelerations. + */ public DifferentialDriveWheelAccelerations() {} /** @@ -55,8 +58,8 @@ public DifferentialDriveWheelAccelerations(LinearAcceleration left, LinearAccele /** * Adds two DifferentialDriveWheelAccelerations and returns the sum. * - *

For example, DifferentialDriveWheelAccelerations{1.0, 0.5} + DifferentialDriveWheelAccelerations{2.0, 1.5} - * = DifferentialDriveWheelAccelerations{3.0, 2.0} + *

For example, DifferentialDriveWheelAccelerations{1.0, 0.5} + + * DifferentialDriveWheelAccelerations{2.0, 1.5} = DifferentialDriveWheelAccelerations{3.0, 2.0} * * @param other The DifferentialDriveWheelAccelerations to add. * @return The sum of the DifferentialDriveWheelAccelerations. @@ -66,11 +69,11 @@ public DifferentialDriveWheelAccelerations plus(DifferentialDriveWheelAccelerati } /** - * Subtracts the other DifferentialDriveWheelAccelerations from the current DifferentialDriveWheelAccelerations - * and returns the difference. + * Subtracts the other DifferentialDriveWheelAccelerations from the current + * DifferentialDriveWheelAccelerations and returns the difference. * - *

For example, DifferentialDriveWheelAccelerations{5.0, 4.0} - DifferentialDriveWheelAccelerations{1.0, 2.0} - * = DifferentialDriveWheelAccelerations{4.0, 2.0} + *

For example, DifferentialDriveWheelAccelerations{5.0, 4.0} - + * DifferentialDriveWheelAccelerations{1.0, 2.0} = DifferentialDriveWheelAccelerations{4.0, 2.0} * * @param other The DifferentialDriveWheelAccelerations to subtract. * @return The difference between the two DifferentialDriveWheelAccelerations. @@ -80,8 +83,8 @@ public DifferentialDriveWheelAccelerations minus(DifferentialDriveWheelAccelerat } /** - * Returns the inverse of the current DifferentialDriveWheelAccelerations. This is equivalent to negating - * all components of the DifferentialDriveWheelAccelerations. + * Returns the inverse of the current DifferentialDriveWheelAccelerations. This is equivalent to + * negating all components of the DifferentialDriveWheelAccelerations. * * @return The inverse of the current DifferentialDriveWheelAccelerations. */ @@ -93,8 +96,8 @@ public DifferentialDriveWheelAccelerations unaryMinus() { * Multiplies the DifferentialDriveWheelAccelerations by a scalar and returns the new * DifferentialDriveWheelAccelerations. * - *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2 = DifferentialDriveWheelAccelerations{4.0, - * 5.0} + *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2 = + * DifferentialDriveWheelAccelerations{4.0, 5.0} * * @param scalar The scalar to multiply by. * @return The scaled DifferentialDriveWheelAccelerations. @@ -107,8 +110,8 @@ public DifferentialDriveWheelAccelerations times(double scalar) { * Divides the DifferentialDriveWheelAccelerations by a scalar and returns the new * DifferentialDriveWheelAccelerations. * - *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2 = DifferentialDriveWheelAccelerations{1.0, - * 1.25} + *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2 = + * DifferentialDriveWheelAccelerations{1.0, 1.25} * * @param scalar The scalar to divide by. * @return The scaled DifferentialDriveWheelAccelerations. diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java index 02da767f434..19d66536157 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java @@ -27,10 +27,12 @@ public class MecanumDriveWheelAccelerations implements ProtobufSerializable, Str public double rearRight; /** MecanumDriveWheelAccelerations protobuf for serialization. */ - public static final MecanumDriveWheelAccelerationsProto proto = new MecanumDriveWheelAccelerationsProto(); + public static final MecanumDriveWheelAccelerationsProto proto = + new MecanumDriveWheelAccelerationsProto(); /** MecanumDriveWheelAccelerations struct for serialization. */ - public static final MecanumDriveWheelAccelerationsStruct struct = new MecanumDriveWheelAccelerationsStruct(); + public static final MecanumDriveWheelAccelerationsStruct struct = + new MecanumDriveWheelAccelerationsStruct(); /** Constructs a MecanumDriveWheelAccelerations with zeros for all member fields. */ public MecanumDriveWheelAccelerations() {} @@ -74,8 +76,9 @@ public MecanumDriveWheelAccelerations( /** * Adds two MecanumDriveWheelAccelerations and returns the sum. * - *

For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} + MecanumDriveWheelAccelerations{2.0, 1.5, - * 0.5, 1.0} = MecanumDriveWheelAccelerations{3.0, 2.0, 2.5, 2.5} + *

For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} + + * MecanumDriveWheelAccelerations{2.0, 1.5, 0.5, 1.0} = MecanumDriveWheelAccelerations{3.0, 2.0, + * 2.5, 2.5} * * @param other The MecanumDriveWheelAccelerations to add. * @return The sum of the MecanumDriveWheelAccelerations. @@ -89,11 +92,12 @@ public MecanumDriveWheelAccelerations plus(MecanumDriveWheelAccelerations other) } /** - * Subtracts the other MecanumDriveWheelAccelerations from the current MecanumDriveWheelAccelerations and - * returns the difference. + * Subtracts the other MecanumDriveWheelAccelerations from the current + * MecanumDriveWheelAccelerations and returns the difference. * - *

For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} - MecanumDriveWheelAccelerations{1.0, 2.0, - * 3.0, 0.5} = MecanumDriveWheelAccelerations{4.0, 2.0, 3.0, 2.0} + *

For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} - + * MecanumDriveWheelAccelerations{1.0, 2.0, 3.0, 0.5} = MecanumDriveWheelAccelerations{4.0, 2.0, + * 3.0, 2.0} * * @param other The MecanumDriveWheelAccelerations to subtract. * @return The difference between the two MecanumDriveWheelAccelerations. @@ -107,8 +111,8 @@ public MecanumDriveWheelAccelerations minus(MecanumDriveWheelAccelerations other } /** - * Returns the inverse of the current MecanumDriveWheelAccelerations. This is equivalent to negating all - * components of the MecanumDriveWheelAccelerations. + * Returns the inverse of the current MecanumDriveWheelAccelerations. This is equivalent to + * negating all components of the MecanumDriveWheelAccelerations. * * @return The inverse of the current MecanumDriveWheelAccelerations. */ @@ -117,10 +121,11 @@ public MecanumDriveWheelAccelerations unaryMinus() { } /** - * Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the new MecanumDriveWheelAccelerations. + * Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the new + * MecanumDriveWheelAccelerations. * - *

For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 = MecanumDriveWheelAccelerations{4.0, - * 5.0, 6.0, 7.0} + *

For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 = + * MecanumDriveWheelAccelerations{4.0, 5.0, 6.0, 7.0} * * @param scalar The scalar to multiply by. * @return The scaled MecanumDriveWheelAccelerations. @@ -131,10 +136,11 @@ public MecanumDriveWheelAccelerations times(double scalar) { } /** - * Divides the MecanumDriveWheelAccelerations by a scalar and returns the new MecanumDriveWheelAccelerations. + * Divides the MecanumDriveWheelAccelerations by a scalar and returns the new + * MecanumDriveWheelAccelerations. * - *

For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 = MecanumDriveWheelAccelerations{1.0, - * 1.25, 0.75, 0.5} + *

For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 = + * MecanumDriveWheelAccelerations{1.0, 1.25, 0.75, 0.5} * * @param scalar The scalar to divide by. * @return The scaled MecanumDriveWheelAccelerations. diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java index 6d292a8cfae..1cdfcecb640 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java @@ -28,16 +28,20 @@ public class SwerveModuleAccelerations public static final SwerveModuleAccelerationsProto proto = new SwerveModuleAccelerationsProto(); /** SwerveModuleAccelerations struct for serialization. */ - public static final SwerveModuleAccelerationsStruct struct = new SwerveModuleAccelerationsStruct(); + public static final SwerveModuleAccelerationsStruct struct = + new SwerveModuleAccelerationsStruct(); - /** Constructs a SwerveModuleAccelerations with zeros for acceleration and angular acceleration. */ + /** + * Constructs a SwerveModuleAccelerations with zeros for acceleration and angular acceleration. + */ public SwerveModuleAccelerations() {} /** * Constructs a SwerveModuleAccelerations. * * @param acceleration The acceleration of the wheel of the module in meters per second squared. - * @param angularAcceleration The angular acceleration of the module in radians per second squared. + * @param angularAcceleration The angular acceleration of the module in radians per second + * squared. */ public SwerveModuleAccelerations(double acceleration, double angularAcceleration) { this.acceleration = acceleration; diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java index dc0c91ac961..77e2547c43d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java @@ -10,7 +10,8 @@ import us.hebi.quickbuf.Descriptors.Descriptor; public class DifferentialDriveWheelAccelerationsProto - implements Protobuf { + implements Protobuf< + DifferentialDriveWheelAccelerations, ProtobufDifferentialDriveWheelAccelerations> { @Override public Class getTypeClass() { return DifferentialDriveWheelAccelerations.class; @@ -27,12 +28,14 @@ public ProtobufDifferentialDriveWheelAccelerations createMessage() { } @Override - public DifferentialDriveWheelAccelerations unpack(ProtobufDifferentialDriveWheelAccelerations msg) { + public DifferentialDriveWheelAccelerations unpack( + ProtobufDifferentialDriveWheelAccelerations msg) { return new DifferentialDriveWheelAccelerations(msg.getLeft(), msg.getRight()); } @Override - public void pack(ProtobufDifferentialDriveWheelAccelerations msg, DifferentialDriveWheelAccelerations value) { + public void pack( + ProtobufDifferentialDriveWheelAccelerations msg, DifferentialDriveWheelAccelerations value) { msg.setLeft(value.left); msg.setRight(value.right); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java index adb1cc89839..c72869f670a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java @@ -33,7 +33,8 @@ public MecanumDriveWheelAccelerations unpack(ProtobufMecanumDriveWheelAccelerati } @Override - public void pack(ProtobufMecanumDriveWheelAccelerations msg, MecanumDriveWheelAccelerations value) { + public void pack( + ProtobufMecanumDriveWheelAccelerations msg, MecanumDriveWheelAccelerations value) { msg.setFrontLeft(value.frontLeft); msg.setFrontRight(value.frontRight); msg.setRearLeft(value.rearLeft); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java index 8dde9cd18e5..46a2861d440 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java @@ -8,7 +8,8 @@ import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; -public class DifferentialDriveWheelAccelerationsStruct implements Struct { +public class DifferentialDriveWheelAccelerationsStruct + implements Struct { @Override public Class getTypeClass() { return DifferentialDriveWheelAccelerations.class; diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java index f84f8b12749..e62d99ff0c0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java @@ -8,7 +8,8 @@ import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; -public class MecanumDriveWheelAccelerationsStruct implements Struct { +public class MecanumDriveWheelAccelerationsStruct + implements Struct { @Override public Class getTypeClass() { return MecanumDriveWheelAccelerations.class; From 760695031f407bd992ccadde07a6ae00b16e1ccd Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 17:38:14 -0400 Subject: [PATCH 10/82] add accelerations to the interface Signed-off-by: Zach Harel --- .../DifferentialDriveKinematics.java | 22 ++- .../wpi/first/math/kinematics/Kinematics.java | 30 ++++- .../kinematics/MecanumDriveKinematics.java | 85 +++++++++++- .../kinematics/SwerveDriveKinematics.java | 127 +++++++++++++++++- 4 files changed, 257 insertions(+), 7 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java index eced997b242..1379465625f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java @@ -23,7 +23,10 @@ * chassis speed. */ public class DifferentialDriveKinematics - implements Kinematics, + implements Kinematics< + DifferentialDriveWheelSpeeds, + DifferentialDriveWheelAccelerations, + DifferentialDriveWheelPositions>, ProtobufSerializable, StructSerializable { /** Differential drive trackwidth in meters. */ @@ -88,6 +91,23 @@ public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega); } + @Override + public ChassisAccelerations toChassisAccelerations( + DifferentialDriveWheelAccelerations wheelAccelerations) { + return new ChassisAccelerations( + (wheelAccelerations.left + wheelAccelerations.right) / 2, + 0.0, + (wheelAccelerations.right - wheelAccelerations.left) / trackwidth); + } + + @Override + public DifferentialDriveWheelAccelerations toWheelAccelerations( + ChassisAccelerations chassisAccelerations) { + return new DifferentialDriveWheelAccelerations( + chassisAccelerations.ax - trackwidth / 2 * chassisAccelerations.alpha, + chassisAccelerations.ax + trackwidth / 2 * chassisAccelerations.alpha); + } + @Override public Twist2d toTwist2d( DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java index 307999d6552..df988dc4f14 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java @@ -8,14 +8,15 @@ import edu.wpi.first.math.interpolation.Interpolator; /** - * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot - * code should not use this directly- Instead, use the particular type for your drivetrain (e.g., - * {@link DifferentialDriveKinematics}). + * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds and + * chassis accelerations into wheel accelerations. Robot code should not use this directly- Instead, + * use the particular type for your drivetrain (e.g., {@link DifferentialDriveKinematics}). * * @param The type of the wheel speeds. + * @param The type of the wheel accelerations. * @param

The type of the wheel positions. */ -public interface Kinematics extends Interpolator

{ +public interface Kinematics extends Interpolator

{ /** * Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This * method is often used for odometry -- determining the robot's position on the field using data @@ -35,6 +36,27 @@ public interface Kinematics extends Interpolator

{ */ S toWheelSpeeds(ChassisSpeeds chassisSpeeds); + /** + * Performs forward kinematics to return the resulting chassis accelerations from the wheel + * accelerations. This method is often used for dynamics calculations -- determining the robot's + * acceleration on the field using data from the real-world acceleration of each wheel on the + * robot. + * + * @param wheelAccelerations The accelerations of the wheels. + * @return The chassis accelerations. + */ + ChassisAccelerations toChassisAccelerations(A wheelAccelerations); + + /** + * Performs inverse kinematics to return the wheel accelerations from a desired chassis + * acceleration. This method is often used for dynamics calculations -- converting desired robot + * accelerations into individual wheel accelerations. + * + * @param chassisAccelerations The desired chassis accelerations. + * @return The wheel accelerations. + */ + A toWheelAccelerations(ChassisAccelerations chassisAccelerations); + /** * Performs forward kinematics to return the resulting Twist2d from the given change in wheel * positions. This method is often used for odometry -- determining the robot's position on the diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index 1217f02dd41..5d09d7a2d31 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -34,7 +34,8 @@ * field using encoders and a gyro. */ public class MecanumDriveKinematics - implements Kinematics, + implements Kinematics< + MecanumDriveWheelSpeeds, MecanumDriveWheelAccelerations, MecanumDriveWheelPositions>, ProtobufSerializable, StructSerializable { private final SimpleMatrix m_inverseKinematics; @@ -162,6 +163,88 @@ public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) { chassisSpeedsVector.get(2, 0)); } + /** + * Performs inverse kinematics to return the wheel accelerations from a desired chassis + * acceleration. This method is often used for dynamics calculations -- converting desired robot + * accelerations into individual wheel accelerations. + * + *

This function also supports variable centers of rotation. During normal operations, the + * center of rotation is usually the same as the physical center of the robot; therefore, the + * argument is defaulted to that use case. However, if you wish to change the center of rotation + * for evasive maneuvers, vision alignment, or for any other use case, you can do so. + * + * @param chassisAccelerations The desired chassis accelerations. + * @param centerOfRotation The center of rotation. For example, if you set the center of rotation + * at one corner of the robot and provide a chassis acceleration that only has a dtheta + * component, the robot will rotate around that corner. + * @return The wheel accelerations. + */ + public MecanumDriveWheelAccelerations toWheelAccelerations( + ChassisAccelerations chassisAccelerations, Translation2d centerOfRotation) { + // We have a new center of rotation. We need to compute the matrix again. + if (!centerOfRotation.equals(m_prevCoR)) { + var fl = m_frontLeftWheel.minus(centerOfRotation); + var fr = m_frontRightWheel.minus(centerOfRotation); + var rl = m_rearLeftWheel.minus(centerOfRotation); + var rr = m_rearRightWheel.minus(centerOfRotation); + + setInverseKinematics(fl, fr, rl, rr); + m_prevCoR = centerOfRotation; + } + + var chassisAccelerationsVector = new SimpleMatrix(3, 1); + chassisAccelerationsVector.setColumn( + 0, 0, chassisAccelerations.ax, chassisAccelerations.ay, chassisAccelerations.alpha); + + var wheelsVector = m_inverseKinematics.mult(chassisAccelerationsVector); + return new MecanumDriveWheelAccelerations( + wheelsVector.get(0, 0), + wheelsVector.get(1, 0), + wheelsVector.get(2, 0), + wheelsVector.get(3, 0)); + } + + /** + * Performs inverse kinematics. See {@link #toWheelAccelerations(ChassisAccelerations, + * Translation2d)} for more information. + * + * @param chassisAccelerations The desired chassis accelerations. + * @return The wheel accelerations. + */ + @Override + public MecanumDriveWheelAccelerations toWheelAccelerations( + ChassisAccelerations chassisAccelerations) { + return toWheelAccelerations(chassisAccelerations, Translation2d.kZero); + } + + /** + * Performs forward kinematics to return the resulting chassis accelerations from the given wheel + * accelerations. This method is often used for dynamics calculations -- determining the robot's + * acceleration on the field using data from the real-world acceleration of each wheel on the + * robot. + * + * @param wheelAccelerations The current mecanum drive wheel accelerations. + * @return The resulting chassis accelerations. + */ + @Override + public ChassisAccelerations toChassisAccelerations( + MecanumDriveWheelAccelerations wheelAccelerations) { + var wheelAccelerationsVector = new SimpleMatrix(4, 1); + wheelAccelerationsVector.setColumn( + 0, + 0, + wheelAccelerations.frontLeft, + wheelAccelerations.frontRight, + wheelAccelerations.rearLeft, + wheelAccelerations.rearRight); + var chassisAccelerationsVector = m_forwardKinematics.mult(wheelAccelerationsVector); + + return new ChassisAccelerations( + chassisAccelerationsVector.get(0, 0), + chassisAccelerationsVector.get(1, 0), + chassisAccelerationsVector.get(2, 0)); + } + @Override public Twist2d toTwist2d(MecanumDriveWheelPositions start, MecanumDriveWheelPositions end) { var wheelDeltasVector = new SimpleMatrix(4, 1); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index 2e69777af85..bab35cb7bd0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -43,7 +43,7 @@ */ @SuppressWarnings("overrides") public class SwerveDriveKinematics - implements Kinematics, + implements Kinematics, ProtobufSerializable, StructSerializable { private final SimpleMatrix m_inverseKinematics; @@ -441,4 +441,129 @@ public Translation2d[] getModules() { public static final SwerveDriveKinematicsStruct getStruct(int numModules) { return new SwerveDriveKinematicsStruct(numModules); } + + /** + * Performs inverse kinematics to return the module accelerations from a desired chassis + * acceleration. This method is often used for dynamics calculations -- converting desired robot + * accelerations into individual module accelerations. + * + *

This function also supports variable centers of rotation. During normal operations, the + * center of rotation is usually the same as the physical center of the robot; therefore, the + * argument is defaulted to that use case. However, if you wish to change the center of rotation + * for evasive maneuvers, vision alignment, or for any other use case, you can do so. + * + *

In the case that the desired chassis accelerations are zero (i.e. the robot will be + * stationary), the previously calculated module angle will be maintained. + * + * @param chassisAccelerations The desired chassis accelerations. + * @param centerOfRotation The center of rotation. For example, if you set the center of rotation + * at one corner of the robot and provide a chassis acceleration that only has a dtheta + * component, the robot will rotate around that corner. + * @return An array containing the module accelerations. + */ + public SwerveModuleAccelerations[] toSwerveModuleAccelerations( + ChassisAccelerations chassisAccelerations, Translation2d centerOfRotation) { + var moduleAccelerations = new SwerveModuleAccelerations[m_numModules]; + + if (chassisAccelerations.ax == 0.0 + && chassisAccelerations.ay == 0.0 + && chassisAccelerations.alpha == 0.0) { + for (int i = 0; i < m_numModules; i++) { + moduleAccelerations[i] = new SwerveModuleAccelerations(0.0, 0.0); + } + return moduleAccelerations; + } + + if (!centerOfRotation.equals(m_prevCoR)) { + for (int i = 0; i < m_numModules; i++) { + m_inverseKinematics.setRow( + i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotation.getY()); + m_inverseKinematics.setRow( + i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX() - centerOfRotation.getX()); + } + m_prevCoR = centerOfRotation; + } + + var chassisAccelerationsVector = new SimpleMatrix(3, 1); + chassisAccelerationsVector.setColumn( + 0, 0, chassisAccelerations.ax, chassisAccelerations.ay, chassisAccelerations.alpha); + + var moduleAccelerationsMatrix = m_inverseKinematics.mult(chassisAccelerationsVector); + + for (int i = 0; i < m_numModules; i++) { + double x = moduleAccelerationsMatrix.get(i * 2, 0); + double y = moduleAccelerationsMatrix.get(i * 2 + 1, 0); + + // For swerve modules, we need to compute both linear acceleration and angular acceleration + // The linear acceleration is the magnitude of the acceleration vector + double linearAcceleration = Math.hypot(x, y); + + // Angular acceleration represents how fast the module angle is changing + // This would typically come from higher-level control algorithms + // For basic kinematics, we set it to 0 (constant steering angle) + double angularAcceleration = 0.0; + + moduleAccelerations[i] = + new SwerveModuleAccelerations(linearAcceleration, angularAcceleration); + } + + return moduleAccelerations; + } + + /** + * Performs inverse kinematics. See {@link #toSwerveModuleAccelerations(ChassisAccelerations, + * Translation2d)} toSwerveModuleAccelerations for more information. + * + * @param chassisAccelerations The desired chassis accelerations. + * @return An array containing the module accelerations. + */ + public SwerveModuleAccelerations[] toSwerveModuleAccelerations( + ChassisAccelerations chassisAccelerations) { + return toSwerveModuleAccelerations(chassisAccelerations, Translation2d.kZero); + } + + @Override + public SwerveModuleAccelerations[] toWheelAccelerations( + ChassisAccelerations chassisAccelerations) { + return toSwerveModuleAccelerations(chassisAccelerations); + } + + /** + * Performs forward kinematics to return the resulting chassis accelerations from the given module + * accelerations. This method is often used for dynamics calculations -- determining the robot's + * acceleration on the field using data from the real-world acceleration of each module on the + * robot. + * + * @param moduleAccelerations The accelerations of the modules as measured from respective + * encoders and gyros. The order of the swerve module accelerations should be same as passed + * into the constructor of this class. + * @return The resulting chassis accelerations. + */ + @Override + public ChassisAccelerations toChassisAccelerations( + SwerveModuleAccelerations... moduleAccelerations) { + if (moduleAccelerations.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of module locations provided in " + + "constructor"); + } + var moduleAccelerationsMatrix = new SimpleMatrix(m_numModules * 2, 1); + + for (int i = 0; i < m_numModules; i++) { + var module = moduleAccelerations[i]; + // For forward kinematics, we need to know the current steering angle of each module + // Since we don't have it directly in the accelerations, we'll use the stored headings + // In practice, this would come from the current module state + Rotation2d moduleAngle = m_moduleHeadings[i]; + + moduleAccelerationsMatrix.set(i * 2, 0, module.acceleration * moduleAngle.getCos()); + moduleAccelerationsMatrix.set(i * 2 + 1, module.acceleration * moduleAngle.getSin()); + } + + var chassisAccelerationsVector = m_forwardKinematics.mult(moduleAccelerationsMatrix); + return new ChassisAccelerations( + chassisAccelerationsVector.get(0, 0), + chassisAccelerationsVector.get(1, 0), + chassisAccelerationsVector.get(2, 0)); + } } From 998fc49c44c40fdcd1d4c1dcce5d8e9bad97653f Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 17:39:53 -0400 Subject: [PATCH 11/82] fix: update Kinematics type parameters in Odometry and PoseEstimator classes to include new third generic parameter Signed-off-by: Zach Harel --- .../main/java/edu/wpi/first/math/estimator/PoseEstimator.java | 2 +- .../java/edu/wpi/first/math/estimator/PoseEstimator3d.java | 2 +- .../src/main/java/edu/wpi/first/math/kinematics/Odometry.java | 4 ++-- .../main/java/edu/wpi/first/math/kinematics/Odometry3d.java | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 2cfe982ce34..00c054f3e6e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -67,7 +67,7 @@ public class PoseEstimator { */ @SuppressWarnings("PMD.UnusedFormalParameter") public PoseEstimator( - Kinematics kinematics, + Kinematics kinematics, Odometry odometry, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java index 31c97549ff5..69a12c01b7a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -75,7 +75,7 @@ public class PoseEstimator3d { */ @SuppressWarnings("PMD.UnusedFormalParameter") public PoseEstimator3d( - Kinematics kinematics, + Kinematics kinematics, Odometry3d odometry, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java index 9c99696590a..ee2dbed1a73 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java @@ -20,7 +20,7 @@ * @param Wheel positions type. */ public class Odometry { - private final Kinematics m_kinematics; + private final Kinematics m_kinematics; private Pose2d m_pose; private Rotation2d m_gyroOffset; @@ -36,7 +36,7 @@ public class Odometry { * @param initialPose The starting position of the robot on the field. */ public Odometry( - Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) { + Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) { m_kinematics = kinematics; m_pose = initialPose; m_gyroOffset = m_pose.getRotation().minus(gyroAngle); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java index a50a7f1d739..1424e17c99d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java @@ -29,7 +29,7 @@ * @param Wheel positions type. */ public class Odometry3d { - private final Kinematics m_kinematics; + private final Kinematics m_kinematics; private Pose3d m_pose; private Rotation3d m_gyroOffset; @@ -45,7 +45,7 @@ public class Odometry3d { * @param initialPose The starting position of the robot on the field. */ public Odometry3d( - Kinematics kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) { + Kinematics kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) { m_kinematics = kinematics; m_pose = initialPose; m_gyroOffset = m_pose.getRotation().minus(gyroAngle); From cb078b521fd37e19388a117e29e0175ae68f25ab Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 17:57:21 -0400 Subject: [PATCH 12/82] refactor: remove unused methods from ChassisAccelerations class Signed-off-by: Zach Harel --- .../math/kinematics/ChassisAccelerations.java | 40 ------------------- 1 file changed, 40 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java index bd8e10b7f21..129d2551c08 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java @@ -79,46 +79,6 @@ public ChassisAccelerations( alpha.in(RadiansPerSecondPerSecond)); } - /** - * Creates a Twist2d from ChassisAccelerations. - * - * @param dt The duration of the timestep in seconds. - * @return Twist2d. - */ - public Twist2d toTwist2d(double dt) { - return new Twist2d(ax * dt, ay * dt, alpha * dt); - } - - /** - * Discretizes a continuous-time chassis speed. - * - *

This function converts this continuous-time chassis speed into a discrete-time one such that - * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the - * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt - * along the y-axis, and alpha * dt around the z-axis). - * - *

This is useful for compensating for translational skew when translating and rotating a - * holonomic (swerve or mecanum) drivetrain. However, scaling down the ChassisAccelerations after - * discretizing (e.g., when desaturating swerve module speeds) rotates the direction of net motion - * in the opposite direction of rotational velocity, introducing a different translational skew - * which is not accounted for by discretization. - * - * @param dt The duration of the timestep in seconds the speeds should be applied for. - * @return Discretized ChassisAccelerations. - */ - public ChassisAccelerations discretize(double dt) { - // Construct the desired pose after a timestep, relative to the current pose. The desired pose - // has decoupled translation and rotation. - var desiredDeltaPose = new Pose2d(ax * dt, ay * dt, new Rotation2d(alpha * dt)); - - // Find the chassis translation/rotation deltas in the robot frame that move the robot from its - // current pose to the desired pose - var twist = Pose2d.kZero.log(desiredDeltaPose); - - // Turn the chassis translation/rotation deltas into average velocities - return new ChassisAccelerations(twist.dx / dt, twist.dy / dt, twist.dtheta / dt); - } - /** * Converts this field-relative set of speeds into a robot-relative ChassisAccelerations object. * From 02da2639c757ea392af9c8f70cd73ebee11a3d4d Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 17:57:29 -0400 Subject: [PATCH 13/82] test: add unit tests for ChassisAccelerations and related classes Signed-off-by: Zach Harel --- .../kinematics/ChassisAccelerationsTest.java | 160 ++++++++++++++++++ .../math/kinematics/ChassisSpeedsTest.java | 32 ++++ ...fferentialDriveWheelAccelerationsTest.java | 95 +++++++++++ .../MecanumDriveWheelAccelerationsTest.java | 113 +++++++++++++ .../SwerveModuleAccelerationsTest.java | 118 +++++++++++++ 5 files changed, 518 insertions(+) create mode 100644 wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisAccelerationsTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerationsTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerationsTest.java create mode 100644 wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisAccelerationsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisAccelerationsTest.java new file mode 100644 index 00000000000..6445ba82536 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisAccelerationsTest.java @@ -0,0 +1,160 @@ +// 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; + +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.junit.jupiter.api.Test; + +class ChassisAccelerationsTest { + private static final double kEpsilon = 1E-9; + + @Test + void testDefaultConstructor() { + var accelerations = new ChassisAccelerations(); + + assertAll( + () -> assertEquals(0.0, accelerations.ax, kEpsilon), + () -> assertEquals(0.0, accelerations.ay, kEpsilon), + () -> assertEquals(0.0, accelerations.alpha, kEpsilon)); + } + + @Test + void testParameterizedConstructor() { + var accelerations = new ChassisAccelerations(1.0, 2.0, 3.0); + + assertAll( + () -> assertEquals(1.0, accelerations.ax, kEpsilon), + () -> assertEquals(2.0, accelerations.ay, kEpsilon), + () -> assertEquals(3.0, accelerations.alpha, kEpsilon)); + } + + @Test + void testMeasureConstructor() { + var ax = MetersPerSecondPerSecond.of(2.5); + var ay = MetersPerSecondPerSecond.of(1.5); + var alpha = RadiansPerSecondPerSecond.of(0.75); + var accelerations = new ChassisAccelerations(ax, ay, alpha); + + assertAll( + () -> assertEquals(2.5, accelerations.ax, kEpsilon), + () -> assertEquals(1.5, accelerations.ay, kEpsilon), + () -> assertEquals(0.75, accelerations.alpha, kEpsilon)); + } + + @Test + void testToRobotRelative() { + final var chassisAccelerations = + new ChassisAccelerations(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2); + + assertAll( + () -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon), + () -> assertEquals(1.0, chassisAccelerations.ay, kEpsilon), + () -> assertEquals(0.5, chassisAccelerations.alpha, kEpsilon)); + } + + @Test + void testToFieldRelative() { + final var chassisAccelerations = + new ChassisAccelerations(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0)); + + assertAll( + () -> assertEquals(1.0 / Math.sqrt(2.0), chassisAccelerations.ax, kEpsilon), + () -> assertEquals(1.0 / Math.sqrt(2.0), chassisAccelerations.ay, kEpsilon), + () -> assertEquals(0.5, chassisAccelerations.alpha, kEpsilon)); + } + + @Test + void testPlus() { + final var left = new ChassisAccelerations(1.0, 0.5, 0.75); + final var right = new ChassisAccelerations(2.0, 1.5, 0.25); + + final var chassisAccelerations = left.plus(right); + + assertAll( + () -> assertEquals(3.0, chassisAccelerations.ax), + () -> assertEquals(2.0, chassisAccelerations.ay), + () -> assertEquals(1.0, chassisAccelerations.alpha)); + } + + @Test + void testMinus() { + final var left = new ChassisAccelerations(1.0, 0.5, 0.75); + final var right = new ChassisAccelerations(2.0, 0.5, 0.25); + + final var chassisAccelerations = left.minus(right); + + assertAll( + () -> assertEquals(-1.0, chassisAccelerations.ax), + () -> assertEquals(0.0, chassisAccelerations.ay), + () -> assertEquals(0.5, chassisAccelerations.alpha)); + } + + @Test + void testUnaryMinus() { + final var chassisAccelerations = new ChassisAccelerations(1.0, 0.5, 0.75).unaryMinus(); + + assertAll( + () -> assertEquals(-1.0, chassisAccelerations.ax), + () -> assertEquals(-0.5, chassisAccelerations.ay), + () -> assertEquals(-0.75, chassisAccelerations.alpha)); + } + + @Test + void testMultiplication() { + final var chassisAccelerations = new ChassisAccelerations(1.0, 0.5, 0.75).times(2.0); + + assertAll( + () -> assertEquals(2.0, chassisAccelerations.ax), + () -> assertEquals(1.0, chassisAccelerations.ay), + () -> assertEquals(1.5, chassisAccelerations.alpha)); + } + + @Test + void testDivision() { + final var chassisAccelerations = new ChassisAccelerations(2.0, 1.0, 1.5).div(2.0); + + assertAll( + () -> assertEquals(1.0, chassisAccelerations.ax), + () -> assertEquals(0.5, chassisAccelerations.ay), + () -> assertEquals(0.75, chassisAccelerations.alpha)); + } + + @Test + void testInterpolation() { + var start = new ChassisAccelerations(0.0, 0.0, 0.0); + var end = new ChassisAccelerations(10.0, 20.0, 30.0); + var result = start.interpolate(end, 0.5); + + assertAll( + () -> assertEquals(5.0, result.ax, kEpsilon), + () -> assertEquals(10.0, result.ay, kEpsilon), + () -> assertEquals(15.0, result.alpha, kEpsilon)); + } + + @Test + void testInterpolationAtBounds() { + var start = new ChassisAccelerations(1.0, 2.0, 3.0); + var end = new ChassisAccelerations(4.0, 5.0, 6.0); + + // Test t = 0 (should return start) + var resultStart = start.interpolate(end, 0.0); + assertAll( + () -> assertEquals(1.0, resultStart.ax, kEpsilon), + () -> assertEquals(2.0, resultStart.ay, kEpsilon), + () -> assertEquals(3.0, resultStart.alpha, kEpsilon)); + + // Test t = 1 (should return end) + var resultEnd = start.interpolate(end, 1.0); + assertAll( + () -> assertEquals(4.0, resultEnd.ax, kEpsilon), + () -> assertEquals(5.0, resultEnd.ay, kEpsilon), + () -> assertEquals(6.0, resultEnd.alpha, kEpsilon)); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index 38ac1ce2498..c7bdb95187d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -127,4 +127,36 @@ void testDivision() { () -> assertEquals(0.25, chassisSpeeds.vy), () -> assertEquals(0.375, chassisSpeeds.omega)); } + + @Test + void testInterpolation() { + var start = new ChassisSpeeds(0.0, 0.0, 0.0); + var end = new ChassisSpeeds(10.0, 20.0, 30.0); + var result = start.interpolate(end, 0.5); + + assertAll( + () -> assertEquals(5.0, result.vx, kEpsilon), + () -> assertEquals(10.0, result.vy, kEpsilon), + () -> assertEquals(15.0, result.omega, kEpsilon)); + } + + @Test + void testInterpolationAtBounds() { + var start = new ChassisSpeeds(1.0, 2.0, 3.0); + var end = new ChassisSpeeds(4.0, 5.0, 6.0); + + // Test t = 0 (should return start) + var resultStart = start.interpolate(end, 0.0); + assertAll( + () -> assertEquals(1.0, resultStart.vx, kEpsilon), + () -> assertEquals(2.0, resultStart.vy, kEpsilon), + () -> assertEquals(3.0, resultStart.omega, kEpsilon)); + + // Test t = 1 (should return end) + var resultEnd = start.interpolate(end, 1.0); + assertAll( + () -> assertEquals(4.0, resultEnd.vx, kEpsilon), + () -> assertEquals(5.0, resultEnd.vy, kEpsilon), + () -> assertEquals(6.0, resultEnd.omega, kEpsilon)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerationsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerationsTest.java new file mode 100644 index 00000000000..f98c30e5efe --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerationsTest.java @@ -0,0 +1,95 @@ +// 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; + +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelAccelerationsTest { + private static final double kEpsilon = 1E-9; + + @Test + void testDefaultConstructor() { + var wheelAccelerations = new DifferentialDriveWheelAccelerations(); + + assertAll( + () -> assertEquals(0.0, wheelAccelerations.left, kEpsilon), + () -> assertEquals(0.0, wheelAccelerations.right, kEpsilon)); + } + + @Test + void testParameterizedConstructor() { + var wheelAccelerations = new DifferentialDriveWheelAccelerations(1.5, 2.5); + + assertAll( + () -> assertEquals(1.5, wheelAccelerations.left, kEpsilon), + () -> assertEquals(2.5, wheelAccelerations.right, kEpsilon)); + } + + @Test + void testMeasureConstructor() { + var left = MetersPerSecondPerSecond.of(3.0); + var right = MetersPerSecondPerSecond.of(4.5); + var wheelAccelerations = new DifferentialDriveWheelAccelerations(left, right); + + assertAll( + () -> assertEquals(3.0, wheelAccelerations.left, kEpsilon), + () -> assertEquals(4.5, wheelAccelerations.right, kEpsilon)); + } + + @Test + void testPlus() { + final var left = new DifferentialDriveWheelAccelerations(1.0, 0.5); + final var right = new DifferentialDriveWheelAccelerations(2.0, 1.5); + + final var wheelAccelerations = left.plus(right); + + assertAll( + () -> assertEquals(3.0, wheelAccelerations.left), + () -> assertEquals(2.0, wheelAccelerations.right)); + } + + @Test + void testMinus() { + final var left = new DifferentialDriveWheelAccelerations(1.0, 0.5); + final var right = new DifferentialDriveWheelAccelerations(2.0, 0.5); + + final var wheelAccelerations = left.minus(right); + + assertAll( + () -> assertEquals(-1.0, wheelAccelerations.left), + () -> assertEquals(0.0, wheelAccelerations.right)); + } + + @Test + void testUnaryMinus() { + final var wheelAccelerations = new DifferentialDriveWheelAccelerations(1.0, 0.5).unaryMinus(); + + assertAll( + () -> assertEquals(-1.0, wheelAccelerations.left), + () -> assertEquals(-0.5, wheelAccelerations.right)); + } + + @Test + void testMultiplication() { + final var wheelAccelerations = new DifferentialDriveWheelAccelerations(1.0, 0.5).times(2.0); + + assertAll( + () -> assertEquals(2.0, wheelAccelerations.left), + () -> assertEquals(1.0, wheelAccelerations.right)); + } + + @Test + void testDivision() { + final var wheelAccelerations = new DifferentialDriveWheelAccelerations(1.0, 0.5).div(2.0); + + assertAll( + () -> assertEquals(0.5, wheelAccelerations.left), + () -> assertEquals(0.25, wheelAccelerations.right)); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerationsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerationsTest.java new file mode 100644 index 00000000000..d19166fde8b --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerationsTest.java @@ -0,0 +1,113 @@ +// 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; + +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +class MecanumDriveWheelAccelerationsTest { + private static final double kEpsilon = 1E-9; + + @Test + void testDefaultConstructor() { + var wheelAccelerations = new MecanumDriveWheelAccelerations(); + + assertAll( + () -> assertEquals(0.0, wheelAccelerations.frontLeft, kEpsilon), + () -> assertEquals(0.0, wheelAccelerations.frontRight, kEpsilon), + () -> assertEquals(0.0, wheelAccelerations.rearLeft, kEpsilon), + () -> assertEquals(0.0, wheelAccelerations.rearRight, kEpsilon)); + } + + @Test + void testParameterizedConstructor() { + var wheelAccelerations = new MecanumDriveWheelAccelerations(1.0, 2.0, 3.0, 4.0); + + assertAll( + () -> assertEquals(1.0, wheelAccelerations.frontLeft, kEpsilon), + () -> assertEquals(2.0, wheelAccelerations.frontRight, kEpsilon), + () -> assertEquals(3.0, wheelAccelerations.rearLeft, kEpsilon), + () -> assertEquals(4.0, wheelAccelerations.rearRight, kEpsilon)); + } + + @Test + void testMeasureConstructor() { + var frontLeft = MetersPerSecondPerSecond.of(1.5); + var frontRight = MetersPerSecondPerSecond.of(2.5); + var rearLeft = MetersPerSecondPerSecond.of(3.5); + var rearRight = MetersPerSecondPerSecond.of(4.5); + var wheelAccelerations = new MecanumDriveWheelAccelerations(frontLeft, frontRight, rearLeft, rearRight); + + assertAll( + () -> assertEquals(1.5, wheelAccelerations.frontLeft, kEpsilon), + () -> assertEquals(2.5, wheelAccelerations.frontRight, kEpsilon), + () -> assertEquals(3.5, wheelAccelerations.rearLeft, kEpsilon), + () -> assertEquals(4.5, wheelAccelerations.rearRight, kEpsilon)); + } + + @Test + void testPlus() { + final var left = new MecanumDriveWheelAccelerations(1.0, 0.5, 2.0, 1.5); + final var right = new MecanumDriveWheelAccelerations(2.0, 1.5, 0.5, 1.0); + + final var wheelAccelerations = left.plus(right); + + assertAll( + () -> assertEquals(3.0, wheelAccelerations.frontLeft), + () -> assertEquals(2.0, wheelAccelerations.frontRight), + () -> assertEquals(2.5, wheelAccelerations.rearLeft), + () -> assertEquals(2.5, wheelAccelerations.rearRight)); + } + + @Test + void testMinus() { + final var left = new MecanumDriveWheelAccelerations(5.0, 4.0, 6.0, 2.5); + final var right = new MecanumDriveWheelAccelerations(1.0, 2.0, 3.0, 0.5); + + final var wheelAccelerations = left.minus(right); + + assertAll( + () -> assertEquals(4.0, wheelAccelerations.frontLeft), + () -> assertEquals(2.0, wheelAccelerations.frontRight), + () -> assertEquals(3.0, wheelAccelerations.rearLeft), + () -> assertEquals(2.0, wheelAccelerations.rearRight)); + } + + @Test + void testUnaryMinus() { + final var wheelAccelerations = new MecanumDriveWheelAccelerations(1.0, -2.0, 3.0, -4.0).unaryMinus(); + + assertAll( + () -> assertEquals(-1.0, wheelAccelerations.frontLeft), + () -> assertEquals(2.0, wheelAccelerations.frontRight), + () -> assertEquals(-3.0, wheelAccelerations.rearLeft), + () -> assertEquals(4.0, wheelAccelerations.rearRight)); + } + + @Test + void testMultiplication() { + final var wheelAccelerations = new MecanumDriveWheelAccelerations(2.0, 2.5, 3.0, 3.5).times(2.0); + + assertAll( + () -> assertEquals(4.0, wheelAccelerations.frontLeft), + () -> assertEquals(5.0, wheelAccelerations.frontRight), + () -> assertEquals(6.0, wheelAccelerations.rearLeft), + () -> assertEquals(7.0, wheelAccelerations.rearRight)); + } + + @Test + void testDivision() { + final var wheelAccelerations = new MecanumDriveWheelAccelerations(2.0, 2.5, 1.5, 1.0).div(2.0); + + assertAll( + () -> assertEquals(1.0, wheelAccelerations.frontLeft), + () -> assertEquals(1.25, wheelAccelerations.frontRight), + () -> assertEquals(0.75, wheelAccelerations.rearLeft), + () -> assertEquals(0.5, wheelAccelerations.rearRight)); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java new file mode 100644 index 00000000000..9e7c9e0cc88 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java @@ -0,0 +1,118 @@ +// 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; + +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +class SwerveModuleAccelerationsTest { + private static final double kEpsilon = 1E-9; + + @Test + void testDefaultConstructor() { + var moduleAccelerations = new SwerveModuleAccelerations(); + + assertAll( + () -> assertEquals(0.0, moduleAccelerations.acceleration, kEpsilon), + () -> assertEquals(0.0, moduleAccelerations.angularAcceleration, kEpsilon)); + } + + @Test + void testParameterizedConstructor() { + var moduleAccelerations = new SwerveModuleAccelerations(2.5, 1.5); + + assertAll( + () -> assertEquals(2.5, moduleAccelerations.acceleration, kEpsilon), + () -> assertEquals(1.5, moduleAccelerations.angularAcceleration, kEpsilon)); + } + + @Test + void testMeasureConstructor() { + var acceleration = MetersPerSecondPerSecond.of(3.0); + var angularAcceleration = RadiansPerSecondPerSecond.of(2.0); + var moduleAccelerations = new SwerveModuleAccelerations(acceleration, angularAcceleration); + + assertAll( + () -> assertEquals(3.0, moduleAccelerations.acceleration, kEpsilon), + () -> assertEquals(2.0, moduleAccelerations.angularAcceleration, kEpsilon)); + } + + @Test + void testEquals() { + var moduleAccelerations1 = new SwerveModuleAccelerations(2.0, 1.5); + var moduleAccelerations2 = new SwerveModuleAccelerations(2.0, 1.5); + var moduleAccelerations3 = new SwerveModuleAccelerations(2.1, 1.5); + + assertTrue(moduleAccelerations1.equals(moduleAccelerations2)); + assertFalse(moduleAccelerations1.equals(moduleAccelerations3)); + } + + @Test + void testCompareTo() { + var slower = new SwerveModuleAccelerations(1.0, 2.0); + var faster = new SwerveModuleAccelerations(2.0, 1.0); + + assertTrue(slower.compareTo(faster) < 0); + assertTrue(faster.compareTo(slower) > 0); + assertEquals(0, slower.compareTo(slower)); + } + + @Test + void testPlus() { + final var left = new SwerveModuleAccelerations(1.0, 0.5); + final var right = new SwerveModuleAccelerations(2.0, 1.5); + + final var moduleAccelerations = left.plus(right); + + assertAll( + () -> assertEquals(3.0, moduleAccelerations.acceleration), + () -> assertEquals(2.0, moduleAccelerations.angularAcceleration)); + } + + @Test + void testMinus() { + final var left = new SwerveModuleAccelerations(3.0, 2.5); + final var right = new SwerveModuleAccelerations(1.0, 0.5); + + final var moduleAccelerations = left.minus(right); + + assertAll( + () -> assertEquals(2.0, moduleAccelerations.acceleration), + () -> assertEquals(2.0, moduleAccelerations.angularAcceleration)); + } + + @Test + void testUnaryMinus() { + final var moduleAccelerations = new SwerveModuleAccelerations(1.5, -2.0).unaryMinus(); + + assertAll( + () -> assertEquals(-1.5, moduleAccelerations.acceleration), + () -> assertEquals(2.0, moduleAccelerations.angularAcceleration)); + } + + @Test + void testMultiplication() { + final var moduleAccelerations = new SwerveModuleAccelerations(2.0, 1.5).times(2.0); + + assertAll( + () -> assertEquals(4.0, moduleAccelerations.acceleration), + () -> assertEquals(3.0, moduleAccelerations.angularAcceleration)); + } + + @Test + void testDivision() { + final var moduleAccelerations = new SwerveModuleAccelerations(4.0, 2.0).div(2.0); + + assertAll( + () -> assertEquals(2.0, moduleAccelerations.acceleration), + () -> assertEquals(1.0, moduleAccelerations.angularAcceleration)); + } +} From 01683b1bbbfd93fc306fc178f13f08f983fdc40a Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 18:01:13 -0400 Subject: [PATCH 14/82] test: add unit tests for ChassisAccelerations and related classes Signed-off-by: Zach Harel --- .../DifferentialDriveKinematicsTest.java | 63 +++++++++++ .../MecanumDriveKinematicsTest.java | 104 ++++++++++++++++++ .../kinematics/SwerveDriveKinematicsTest.java | 61 ++++++++++ 3 files changed, 228 insertions(+) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java index d6a4473e3f7..b2c6257927f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java @@ -76,4 +76,67 @@ void testForwardKinematicsForRotateInPlace() { () -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon), () -> assertEquals(-Math.PI, chassisSpeeds.omega, kEpsilon)); } + + @Test + void testInverseAccelerationsForZeros() { + var chassisAccelerations = new ChassisAccelerations(); + var wheelAccelerations = m_kinematics.toWheelAccelerations(chassisAccelerations); + + assertAll( + () -> assertEquals(0.0, wheelAccelerations.left, kEpsilon), + () -> assertEquals(0.0, wheelAccelerations.right, kEpsilon)); + } + + @Test + void testForwardAccelerationsForZeros() { + var wheelAccelerations = new DifferentialDriveWheelAccelerations(); + var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); + + assertAll( + () -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon), + () -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon), + () -> assertEquals(0.0, chassisAccelerations.alpha, kEpsilon)); + } + + @Test + void testInverseAccelerationsForStraightLine() { + var chassisAccelerations = new ChassisAccelerations(3, 0, 0); + var wheelAccelerations = m_kinematics.toWheelAccelerations(chassisAccelerations); + + assertAll( + () -> assertEquals(3.0, wheelAccelerations.left, kEpsilon), + () -> assertEquals(3.0, wheelAccelerations.right, kEpsilon)); + } + + @Test + void testForwardAccelerationsForStraightLine() { + var wheelAccelerations = new DifferentialDriveWheelAccelerations(3, 3); + var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); + + assertAll( + () -> assertEquals(3.0, chassisAccelerations.ax, kEpsilon), + () -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon), + () -> assertEquals(0.0, chassisAccelerations.alpha, kEpsilon)); + } + + @Test + void testInverseAccelerationsForRotateInPlace() { + var chassisAccelerations = new ChassisAccelerations(0, 0, Math.PI); + var wheelAccelerations = m_kinematics.toWheelAccelerations(chassisAccelerations); + + assertAll( + () -> assertEquals(-0.381 * Math.PI, wheelAccelerations.left, kEpsilon), + () -> assertEquals(+0.381 * Math.PI, wheelAccelerations.right, kEpsilon)); + } + + @Test + void testForwardAccelerationsForRotateInPlace() { + var wheelAccelerations = new DifferentialDriveWheelAccelerations(+0.381 * Math.PI, -0.381 * Math.PI); + var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); + + assertAll( + () -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon), + () -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon), + () -> assertEquals(-Math.PI, chassisAccelerations.alpha, kEpsilon)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java index 73e6f74a003..0bb485ad7f1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java @@ -169,6 +169,110 @@ void testOffCenterRotationInverseKinematics() { () -> assertEquals(48.0, moduleStates.rearRight, 0.1)); } + @Test + void testStraightLineInverseAccelerations() { + ChassisAccelerations accelerations = new ChassisAccelerations(5, 0, 0); + var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations); + + assertAll( + () -> assertEquals(5.0, wheelAccelerations.frontLeft, 0.1), + () -> assertEquals(5.0, wheelAccelerations.frontRight, 0.1), + () -> assertEquals(5.0, wheelAccelerations.rearLeft, 0.1), + () -> assertEquals(5.0, wheelAccelerations.rearRight, 0.1)); + } + + @Test + void testStraightLineForwardAccelerations() { + var wheelAccelerations = new MecanumDriveWheelAccelerations(3.536, 3.536, 3.536, 3.536); + var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); + + assertAll( + () -> assertEquals(3.536, chassisAccelerations.ax, 0.1), + () -> assertEquals(0, chassisAccelerations.ay, 0.1), + () -> assertEquals(0, chassisAccelerations.alpha, 0.1)); + } + + @Test + void testStrafeInverseAccelerations() { + ChassisAccelerations accelerations = new ChassisAccelerations(0, 4, 0); + var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations); + + assertAll( + () -> assertEquals(-4.0, wheelAccelerations.frontLeft, 0.1), + () -> assertEquals(4.0, wheelAccelerations.frontRight, 0.1), + () -> assertEquals(4.0, wheelAccelerations.rearLeft, 0.1), + () -> assertEquals(-4.0, wheelAccelerations.rearRight, 0.1)); + } + + @Test + void testStrafeForwardAccelerations() { + var wheelAccelerations = new MecanumDriveWheelAccelerations(-2.828427, 2.828427, 2.828427, -2.828427); + var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); + + assertAll( + () -> assertEquals(0, chassisAccelerations.ax, 0.1), + () -> assertEquals(2.8284, chassisAccelerations.ay, 0.1), + () -> assertEquals(0, chassisAccelerations.alpha, 0.1)); + } + + @Test + void testRotationInverseAccelerations() { + ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 2 * Math.PI); + var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations); + + assertAll( + () -> assertEquals(-150.79645, wheelAccelerations.frontLeft, 0.1), + () -> assertEquals(150.79645, wheelAccelerations.frontRight, 0.1), + () -> assertEquals(-150.79645, wheelAccelerations.rearLeft, 0.1), + () -> assertEquals(150.79645, wheelAccelerations.rearRight, 0.1)); + } + + @Test + void testRotationForwardAccelerations() { + var wheelAccelerations = new MecanumDriveWheelAccelerations(-150.79645, 150.79645, -150.79645, 150.79645); + var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); + + assertAll( + () -> assertEquals(0, chassisAccelerations.ax, 0.1), + () -> assertEquals(0, chassisAccelerations.ay, 0.1), + () -> assertEquals(2 * Math.PI, chassisAccelerations.alpha, 0.1)); + } + + @Test + void testMixedTranslationRotationInverseAccelerations() { + ChassisAccelerations accelerations = new ChassisAccelerations(2, 3, 1); + var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations); + + assertAll( + () -> assertEquals(-25.0, wheelAccelerations.frontLeft, 0.1), + () -> assertEquals(29.0, wheelAccelerations.frontRight, 0.1), + () -> assertEquals(-19.0, wheelAccelerations.rearLeft, 0.1), + () -> assertEquals(23.0, wheelAccelerations.rearRight, 0.1)); + } + + @Test + void testMixedTranslationRotationForwardAccelerations() { + var wheelAccelerations = new MecanumDriveWheelAccelerations(-17.677670, 20.51, -13.44, 16.26); + var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); + + assertAll( + () -> assertEquals(1.413, chassisAccelerations.ax, 0.1), + () -> assertEquals(2.122, chassisAccelerations.ay, 0.1), + () -> assertEquals(0.707, chassisAccelerations.alpha, 0.1)); + } + + @Test + void testOffCenterRotationInverseAccelerations() { + ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 1); + var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations, m_fl); + + assertAll( + () -> assertEquals(0, wheelAccelerations.frontLeft, 0.1), + () -> assertEquals(24.0, wheelAccelerations.frontRight, 0.1), + () -> assertEquals(-24.0, wheelAccelerations.rearLeft, 0.1), + () -> assertEquals(48.0, wheelAccelerations.rearRight, 0.1)); + } + @Test void testOffCenterRotationForwardKinematicsKinematics() { var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index fc3921f52f4..ad24ca616cc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -406,4 +406,65 @@ void testDesaturateNegativeSpeed() { () -> assertEquals(-1.0, arr[2].speed, kEpsilon), () -> assertEquals(-1.0, arr[3].speed, kEpsilon)); } + + @Test + void testTurnInPlaceInverseAccelerations() { + ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 2 * Math.PI); + var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations); + + /* + The circumference of the wheels about the COR is π * diameter, or 2π * radius + the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels + trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second squared, + our wheels must trace out 1 rotation (or 106.63 inches) per second squared. + */ + + assertAll( + () -> assertEquals(106.63, moduleAccelerations[0].acceleration, 0.1), + () -> assertEquals(106.63, moduleAccelerations[1].acceleration, 0.1), + () -> assertEquals(106.63, moduleAccelerations[2].acceleration, 0.1), + () -> assertEquals(106.63, moduleAccelerations[3].acceleration, 0.1), + () -> assertEquals(0.0, moduleAccelerations[0].angularAcceleration, kEpsilon), + () -> assertEquals(0.0, moduleAccelerations[1].angularAcceleration, kEpsilon), + () -> assertEquals(0.0, moduleAccelerations[2].angularAcceleration, kEpsilon), + () -> assertEquals(0.0, moduleAccelerations[3].angularAcceleration, kEpsilon)); + } + + @Test + void testTurnInPlaceForwardAccelerations() { + SwerveModuleAccelerations flAccel = new SwerveModuleAccelerations(106.629, 0.0); + SwerveModuleAccelerations frAccel = new SwerveModuleAccelerations(106.629, 0.0); + SwerveModuleAccelerations blAccel = new SwerveModuleAccelerations(106.629, 0.0); + SwerveModuleAccelerations brAccel = new SwerveModuleAccelerations(106.629, 0.0); + + // Set the headings to match the turn-in-place configuration + m_kinematics.resetHeadings( + Rotation2d.fromDegrees(135), + Rotation2d.fromDegrees(45), + Rotation2d.fromDegrees(-135), + Rotation2d.fromDegrees(-45)); + + var chassisAccelerations = m_kinematics.toChassisAccelerations(flAccel, frAccel, blAccel, brAccel); + + assertAll( + () -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon), + () -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon), + () -> assertEquals(2 * Math.PI, chassisAccelerations.alpha, 0.1)); + } + + @Test + void testOffCenterRotationInverseAccelerations() { + ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 1); + var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations, m_fl); + + assertAll( + () -> assertEquals(0, moduleAccelerations[0].acceleration, 0.1), + () -> assertEquals(24.0, moduleAccelerations[1].acceleration, 0.1), + () -> assertEquals(24.0, moduleAccelerations[2].acceleration, 0.1), + () -> assertEquals(33.941, moduleAccelerations[3].acceleration, 0.1), + () -> assertEquals(0.0, moduleAccelerations[0].angularAcceleration, kEpsilon), + () -> assertEquals(0.0, moduleAccelerations[1].angularAcceleration, kEpsilon), + () -> assertEquals(0.0, moduleAccelerations[2].angularAcceleration, kEpsilon), + () -> assertEquals(0.0, moduleAccelerations[3].angularAcceleration, kEpsilon)); + } } From a3a488ae85847b35da9c7b975217f2a5b7f0e126 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 19:04:18 -0400 Subject: [PATCH 15/82] add interpolation methods for wheelspeeds and accelerations and test them and also make gradle happy Signed-off-by: Zach Harel --- .../math/kinematics/ChassisAccelerations.java | 2 - .../DifferentialDriveWheelAccelerations.java | 22 ++++++- .../DifferentialDriveWheelSpeeds.java | 22 ++++++- .../MecanumDriveWheelAccelerations.java | 26 +++++++- .../kinematics/MecanumDriveWheelSpeeds.java | 23 ++++++- .../kinematics/SwerveModuleAccelerations.java | 23 ++++++- .../math/kinematics/SwerveModuleState.java | 28 ++++++++- .../DifferentialDriveKinematicsTest.java | 3 +- ...fferentialDriveWheelAccelerationsTest.java | 42 +++++++++++++ .../DifferentialDriveWheelSpeedsTest.java | 40 ++++++++++++ .../MecanumDriveKinematicsTest.java | 6 +- .../MecanumDriveWheelAccelerationsTest.java | 63 ++++++++++++++++++- .../MecanumDriveWheelSpeedsTest.java | 54 ++++++++++++++++ .../kinematics/SwerveDriveKinematicsTest.java | 6 +- .../SwerveModuleAccelerationsTest.java | 48 +++++++++++++- .../kinematics/SwerveModuleStateTest.java | 51 +++++++++++++++ 16 files changed, 440 insertions(+), 19 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java index 129d2551c08..33ccf7a69ac 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java @@ -8,10 +8,8 @@ import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; 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.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.ChassisAccelerationsProto; import edu.wpi.first.math.kinematics.struct.ChassisAccelerationsStruct; diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java index a303ede5624..f51a77c75de 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerations.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelAccelerationsProto; import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelAccelerationsStruct; import edu.wpi.first.units.measure.LinearAcceleration; @@ -14,7 +15,9 @@ /** Represents the wheel accelerations for a differential drive drivetrain. */ public class DifferentialDriveWheelAccelerations - implements ProtobufSerializable, StructSerializable { + implements Interpolatable, + ProtobufSerializable, + StructSerializable { /** Acceleration of the left side of the robot in meters per second squared. */ public double left; @@ -120,6 +123,23 @@ public DifferentialDriveWheelAccelerations div(double scalar) { return new DifferentialDriveWheelAccelerations(left / scalar, right / scalar); } + /** + * Returns the linear interpolation of this DifferentialDriveWheelAccelerations and another. + * + * @param endValue The end value for the interpolation. + * @param t How far between the two values to interpolate. This is clamped to [0, 1]. + * @return The interpolated value. + */ + @Override + public DifferentialDriveWheelAccelerations interpolate( + DifferentialDriveWheelAccelerations endValue, double t) { + // Clamp t to [0, 1] + t = Math.max(0.0, Math.min(1.0, t)); + + return new DifferentialDriveWheelAccelerations( + left + t * (endValue.left - left), right + t * (endValue.right - right)); + } + @Override public String toString() { return String.format( diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java index 60b3bca8f35..e520cfc4e7c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; +import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelSpeedsProto; import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct; import edu.wpi.first.units.measure.LinearVelocity; @@ -13,7 +14,10 @@ import edu.wpi.first.util.struct.StructSerializable; /** Represents the wheel speeds for a differential drive drivetrain. */ -public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, StructSerializable { +public class DifferentialDriveWheelSpeeds + implements Interpolatable, + ProtobufSerializable, + StructSerializable { /** Speed of the left side of the robot in meters per second. */ public double left; @@ -150,6 +154,22 @@ public DifferentialDriveWheelSpeeds div(double scalar) { return new DifferentialDriveWheelSpeeds(left / scalar, right / scalar); } + /** + * Returns the linear interpolation of this DifferentialDriveWheelSpeeds and another. + * + * @param endValue The end value for the interpolation. + * @param t How far between the two values to interpolate. This is clamped to [0, 1]. + * @return The interpolated value. + */ + @Override + public DifferentialDriveWheelSpeeds interpolate(DifferentialDriveWheelSpeeds endValue, double t) { + // Clamp t to [0, 1] + t = Math.max(0.0, Math.min(1.0, t)); + + return new DifferentialDriveWheelSpeeds( + left + t * (endValue.left - left), right + t * (endValue.right - right)); + } + @Override public String toString() { return String.format( diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java index 19d66536157..434484a5150 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerations.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelAccelerationsProto; import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelAccelerationsStruct; import edu.wpi.first.units.measure.LinearAcceleration; @@ -13,7 +14,10 @@ import edu.wpi.first.util.struct.StructSerializable; /** Represents the wheel accelerations for a mecanum drive drivetrain. */ -public class MecanumDriveWheelAccelerations implements ProtobufSerializable, StructSerializable { +public class MecanumDriveWheelAccelerations + implements Interpolatable, + ProtobufSerializable, + StructSerializable { /** Acceleration of the front left wheel in meters per second squared. */ public double frontLeft; @@ -150,6 +154,26 @@ public MecanumDriveWheelAccelerations div(double scalar) { frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar); } + /** + * Returns the linear interpolation of this MecanumDriveWheelAccelerations and another. + * + * @param endValue The end value for the interpolation. + * @param t How far between the two values to interpolate. This is clamped to [0, 1]. + * @return The interpolated value. + */ + @Override + public MecanumDriveWheelAccelerations interpolate( + MecanumDriveWheelAccelerations endValue, double t) { + // Clamp t to [0, 1] + t = Math.max(0.0, Math.min(1.0, t)); + + return new MecanumDriveWheelAccelerations( + frontLeft + t * (endValue.frontLeft - frontLeft), + frontRight + t * (endValue.frontRight - frontRight), + rearLeft + t * (endValue.rearLeft - rearLeft), + rearRight + t * (endValue.rearRight - rearRight)); + } + @Override public String toString() { return String.format( diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java index 11c05c0bd40..ce5a65e5611 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; +import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelSpeedsProto; import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelSpeedsStruct; import edu.wpi.first.units.measure.LinearVelocity; @@ -13,7 +14,8 @@ import edu.wpi.first.util.struct.StructSerializable; /** Represents the wheel speeds for a mecanum drive drivetrain. */ -public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSerializable { +public class MecanumDriveWheelSpeeds + implements Interpolatable, ProtobufSerializable, StructSerializable { /** Speed of the front left wheel in meters per second. */ public double frontLeft; @@ -186,6 +188,25 @@ public MecanumDriveWheelSpeeds div(double scalar) { frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar); } + /** + * Returns the linear interpolation of this MecanumDriveWheelSpeeds and another. + * + * @param endValue The end value for the interpolation. + * @param t How far between the two values to interpolate. This is clamped to [0, 1]. + * @return The interpolated value. + */ + @Override + public MecanumDriveWheelSpeeds interpolate(MecanumDriveWheelSpeeds endValue, double t) { + // Clamp t to [0, 1] + t = Math.max(0.0, Math.min(1.0, t)); + + return new MecanumDriveWheelSpeeds( + frontLeft + t * (endValue.frontLeft - frontLeft), + frontRight + t * (endValue.frontRight - frontRight), + rearLeft + t * (endValue.rearLeft - rearLeft), + rearRight + t * (endValue.rearRight - rearRight)); + } + @Override public String toString() { return String.format( diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java index 1cdfcecb640..707519cc6b8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.SwerveModuleAccelerationsProto; import edu.wpi.first.math.kinematics.struct.SwerveModuleAccelerationsStruct; import edu.wpi.first.units.measure.AngularAcceleration; @@ -17,7 +18,10 @@ /** Represents the accelerations of one swerve module. */ public class SwerveModuleAccelerations - implements Comparable, ProtobufSerializable, StructSerializable { + implements Interpolatable, + Comparable, + ProtobufSerializable, + StructSerializable { /** Acceleration of the wheel of the module in meters per second squared. */ public double acceleration; @@ -116,6 +120,23 @@ public SwerveModuleAccelerations div(double scalar) { return new SwerveModuleAccelerations(acceleration / scalar, angularAcceleration / scalar); } + /** + * Returns the linear interpolation of this SwerveModuleAccelerations and another. + * + * @param endValue The end value for the interpolation. + * @param t How far between the two values to interpolate. This is clamped to [0, 1]. + * @return The interpolated value. + */ + @Override + public SwerveModuleAccelerations interpolate(SwerveModuleAccelerations endValue, double t) { + // Clamp t to [0, 1] + t = Math.max(0.0, Math.min(1.0, t)); + + return new SwerveModuleAccelerations( + acceleration + t * (endValue.acceleration - acceleration), + angularAcceleration + t * (endValue.angularAcceleration - angularAcceleration)); + } + @Override public boolean equals(Object obj) { return obj instanceof SwerveModuleAccelerations other diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java index 8855f980bea..ad2e74cec78 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.SwerveModuleStateProto; import edu.wpi.first.math.kinematics.struct.SwerveModuleStateStruct; import edu.wpi.first.units.measure.LinearVelocity; @@ -16,7 +17,10 @@ /** Represents the state of one swerve module. */ public class SwerveModuleState - implements Comparable, ProtobufSerializable, StructSerializable { + implements Interpolatable, + Comparable, + ProtobufSerializable, + StructSerializable { /** Speed of the wheel of the module in meters per second. */ public double speed; @@ -119,6 +123,28 @@ public static SwerveModuleState optimize( } } + /** + * Returns the linear interpolation of this SwerveModuleState and another. The angle is + * interpolated using the shortest path between the two angles. + * + * @param endValue The end value for the interpolation. + * @param t How far between the two values to interpolate. This is clamped to [0, 1]. + * @return The interpolated value. + */ + @Override + public SwerveModuleState interpolate(SwerveModuleState endValue, double t) { + // Clamp t to [0, 1] + t = Math.max(0.0, Math.min(1.0, t)); + + // Interpolate speed linearly + double interpolatedSpeed = speed + t * (endValue.speed - speed); + + // Interpolate angle using the shortest path + Rotation2d interpolatedAngle = angle.interpolate(endValue.angle, t); + + return new SwerveModuleState(interpolatedSpeed, interpolatedAngle); + } + /** * Scales speed by cosine of angle error. This scales down movement perpendicular to the desired * direction of travel that can occur when modules change directions. This results in smoother diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java index b2c6257927f..9dafca2db78 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java @@ -131,7 +131,8 @@ void testInverseAccelerationsForRotateInPlace() { @Test void testForwardAccelerationsForRotateInPlace() { - var wheelAccelerations = new DifferentialDriveWheelAccelerations(+0.381 * Math.PI, -0.381 * Math.PI); + var wheelAccelerations = + new DifferentialDriveWheelAccelerations(+0.381 * Math.PI, -0.381 * Math.PI); var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); assertAll( diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerationsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerationsTest.java index f98c30e5efe..27cae213577 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerationsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelAccelerationsTest.java @@ -92,4 +92,46 @@ void testDivision() { () -> assertEquals(0.5, wheelAccelerations.left), () -> assertEquals(0.25, wheelAccelerations.right)); } + + @Test + void testInterpolate() { + final var start = new DifferentialDriveWheelAccelerations(1.0, 2.0); + final var end = new DifferentialDriveWheelAccelerations(5.0, 6.0); + + // Test interpolation at t=0 (should return start) + final var atStart = start.interpolate(end, 0.0); + assertAll( + () -> assertEquals(1.0, atStart.left, kEpsilon), + () -> assertEquals(2.0, atStart.right, kEpsilon)); + + // Test interpolation at t=1 (should return end) + final var atEnd = start.interpolate(end, 1.0); + assertAll( + () -> assertEquals(5.0, atEnd.left, kEpsilon), + () -> assertEquals(6.0, atEnd.right, kEpsilon)); + + // Test interpolation at t=0.5 (should return midpoint) + final var atMidpoint = start.interpolate(end, 0.5); + assertAll( + () -> assertEquals(3.0, atMidpoint.left, kEpsilon), + () -> assertEquals(4.0, atMidpoint.right, kEpsilon)); + + // Test interpolation at t=0.25 + final var atQuarter = start.interpolate(end, 0.25); + assertAll( + () -> assertEquals(2.0, atQuarter.left, kEpsilon), + () -> assertEquals(3.0, atQuarter.right, kEpsilon)); + + // Test clamping: t < 0 should clamp to 0 + final var belowRange = start.interpolate(end, -0.5); + assertAll( + () -> assertEquals(1.0, belowRange.left, kEpsilon), + () -> assertEquals(2.0, belowRange.right, kEpsilon)); + + // Test clamping: t > 1 should clamp to 1 + final var aboveRange = start.interpolate(end, 1.5); + assertAll( + () -> assertEquals(5.0, aboveRange.left, kEpsilon), + () -> assertEquals(6.0, aboveRange.right, kEpsilon)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java index d1716d9c67c..73ee407169f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java @@ -55,4 +55,44 @@ void testDivision() { assertAll( () -> assertEquals(0.5, wheelSpeeds.left), () -> assertEquals(0.25, wheelSpeeds.right)); } + + @Test + void testInterpolate() { + final var start = new DifferentialDriveWheelSpeeds(1.0, 2.0); + final var end = new DifferentialDriveWheelSpeeds(5.0, 6.0); + + // Test interpolation at t=0 (should return start) + final var atStart = start.interpolate(end, 0.0); + assertAll( + () -> assertEquals(1.0, atStart.left, 1e-9), () -> assertEquals(2.0, atStart.right, 1e-9)); + + // Test interpolation at t=1 (should return end) + final var atEnd = start.interpolate(end, 1.0); + assertAll( + () -> assertEquals(5.0, atEnd.left, 1e-9), () -> assertEquals(6.0, atEnd.right, 1e-9)); + + // Test interpolation at t=0.5 (should return midpoint) + final var atMidpoint = start.interpolate(end, 0.5); + assertAll( + () -> assertEquals(3.0, atMidpoint.left, 1e-9), + () -> assertEquals(4.0, atMidpoint.right, 1e-9)); + + // Test interpolation at t=0.25 + final var atQuarter = start.interpolate(end, 0.25); + assertAll( + () -> assertEquals(2.0, atQuarter.left, 1e-9), + () -> assertEquals(3.0, atQuarter.right, 1e-9)); + + // Test clamping: t < 0 should clamp to 0 + final var belowRange = start.interpolate(end, -0.5); + assertAll( + () -> assertEquals(1.0, belowRange.left, 1e-9), + () -> assertEquals(2.0, belowRange.right, 1e-9)); + + // Test clamping: t > 1 should clamp to 1 + final var aboveRange = start.interpolate(end, 1.5); + assertAll( + () -> assertEquals(5.0, aboveRange.left, 1e-9), + () -> assertEquals(6.0, aboveRange.right, 1e-9)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java index 0bb485ad7f1..dd1d8f41354 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java @@ -206,7 +206,8 @@ void testStrafeInverseAccelerations() { @Test void testStrafeForwardAccelerations() { - var wheelAccelerations = new MecanumDriveWheelAccelerations(-2.828427, 2.828427, 2.828427, -2.828427); + var wheelAccelerations = + new MecanumDriveWheelAccelerations(-2.828427, 2.828427, 2.828427, -2.828427); var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); assertAll( @@ -229,7 +230,8 @@ void testRotationInverseAccelerations() { @Test void testRotationForwardAccelerations() { - var wheelAccelerations = new MecanumDriveWheelAccelerations(-150.79645, 150.79645, -150.79645, 150.79645); + var wheelAccelerations = + new MecanumDriveWheelAccelerations(-150.79645, 150.79645, -150.79645, 150.79645); var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations); assertAll( diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerationsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerationsTest.java index d19166fde8b..dc4ba534b72 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerationsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelAccelerationsTest.java @@ -41,7 +41,8 @@ void testMeasureConstructor() { var frontRight = MetersPerSecondPerSecond.of(2.5); var rearLeft = MetersPerSecondPerSecond.of(3.5); var rearRight = MetersPerSecondPerSecond.of(4.5); - var wheelAccelerations = new MecanumDriveWheelAccelerations(frontLeft, frontRight, rearLeft, rearRight); + var wheelAccelerations = + new MecanumDriveWheelAccelerations(frontLeft, frontRight, rearLeft, rearRight); assertAll( () -> assertEquals(1.5, wheelAccelerations.frontLeft, kEpsilon), @@ -80,7 +81,8 @@ void testMinus() { @Test void testUnaryMinus() { - final var wheelAccelerations = new MecanumDriveWheelAccelerations(1.0, -2.0, 3.0, -4.0).unaryMinus(); + final var wheelAccelerations = + new MecanumDriveWheelAccelerations(1.0, -2.0, 3.0, -4.0).unaryMinus(); assertAll( () -> assertEquals(-1.0, wheelAccelerations.frontLeft), @@ -91,7 +93,8 @@ void testUnaryMinus() { @Test void testMultiplication() { - final var wheelAccelerations = new MecanumDriveWheelAccelerations(2.0, 2.5, 3.0, 3.5).times(2.0); + final var wheelAccelerations = + new MecanumDriveWheelAccelerations(2.0, 2.5, 3.0, 3.5).times(2.0); assertAll( () -> assertEquals(4.0, wheelAccelerations.frontLeft), @@ -110,4 +113,58 @@ void testDivision() { () -> assertEquals(0.75, wheelAccelerations.rearLeft), () -> assertEquals(0.5, wheelAccelerations.rearRight)); } + + @Test + void testInterpolate() { + final var start = new MecanumDriveWheelAccelerations(1.0, 2.0, 3.0, 4.0); + final var end = new MecanumDriveWheelAccelerations(5.0, 6.0, 7.0, 8.0); + + // Test interpolation at t=0 (should return start) + final var atStart = start.interpolate(end, 0.0); + assertAll( + () -> assertEquals(1.0, atStart.frontLeft, kEpsilon), + () -> assertEquals(2.0, atStart.frontRight, kEpsilon), + () -> assertEquals(3.0, atStart.rearLeft, kEpsilon), + () -> assertEquals(4.0, atStart.rearRight, kEpsilon)); + + // Test interpolation at t=1 (should return end) + final var atEnd = start.interpolate(end, 1.0); + assertAll( + () -> assertEquals(5.0, atEnd.frontLeft, kEpsilon), + () -> assertEquals(6.0, atEnd.frontRight, kEpsilon), + () -> assertEquals(7.0, atEnd.rearLeft, kEpsilon), + () -> assertEquals(8.0, atEnd.rearRight, kEpsilon)); + + // Test interpolation at t=0.5 (should return midpoint) + final var atMidpoint = start.interpolate(end, 0.5); + assertAll( + () -> assertEquals(3.0, atMidpoint.frontLeft, kEpsilon), + () -> assertEquals(4.0, atMidpoint.frontRight, kEpsilon), + () -> assertEquals(5.0, atMidpoint.rearLeft, kEpsilon), + () -> assertEquals(6.0, atMidpoint.rearRight, kEpsilon)); + + // Test interpolation at t=0.25 + final var atQuarter = start.interpolate(end, 0.25); + assertAll( + () -> assertEquals(2.0, atQuarter.frontLeft, kEpsilon), + () -> assertEquals(3.0, atQuarter.frontRight, kEpsilon), + () -> assertEquals(4.0, atQuarter.rearLeft, kEpsilon), + () -> assertEquals(5.0, atQuarter.rearRight, kEpsilon)); + + // Test clamping: t < 0 should clamp to 0 + final var belowRange = start.interpolate(end, -0.5); + assertAll( + () -> assertEquals(1.0, belowRange.frontLeft, kEpsilon), + () -> assertEquals(2.0, belowRange.frontRight, kEpsilon), + () -> assertEquals(3.0, belowRange.rearLeft, kEpsilon), + () -> assertEquals(4.0, belowRange.rearRight, kEpsilon)); + + // Test clamping: t > 1 should clamp to 1 + final var aboveRange = start.interpolate(end, 1.5); + assertAll( + () -> assertEquals(5.0, aboveRange.frontLeft, kEpsilon), + () -> assertEquals(6.0, aboveRange.frontRight, kEpsilon), + () -> assertEquals(7.0, aboveRange.rearLeft, kEpsilon), + () -> assertEquals(8.0, aboveRange.rearRight, kEpsilon)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java index 6080bedb15a..c70c4833456 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java @@ -70,4 +70,58 @@ void testDivision() { () -> assertEquals(1.0, wheelSpeeds.rearLeft), () -> assertEquals(0.75, wheelSpeeds.rearRight)); } + + @Test + void testInterpolate() { + final var start = new MecanumDriveWheelSpeeds(1.0, 2.0, 3.0, 4.0); + final var end = new MecanumDriveWheelSpeeds(5.0, 6.0, 7.0, 8.0); + + // Test interpolation at t=0 (should return start) + final var atStart = start.interpolate(end, 0.0); + assertAll( + () -> assertEquals(1.0, atStart.frontLeft, 1e-9), + () -> assertEquals(2.0, atStart.frontRight, 1e-9), + () -> assertEquals(3.0, atStart.rearLeft, 1e-9), + () -> assertEquals(4.0, atStart.rearRight, 1e-9)); + + // Test interpolation at t=1 (should return end) + final var atEnd = start.interpolate(end, 1.0); + assertAll( + () -> assertEquals(5.0, atEnd.frontLeft, 1e-9), + () -> assertEquals(6.0, atEnd.frontRight, 1e-9), + () -> assertEquals(7.0, atEnd.rearLeft, 1e-9), + () -> assertEquals(8.0, atEnd.rearRight, 1e-9)); + + // Test interpolation at t=0.5 (should return midpoint) + final var atMidpoint = start.interpolate(end, 0.5); + assertAll( + () -> assertEquals(3.0, atMidpoint.frontLeft, 1e-9), + () -> assertEquals(4.0, atMidpoint.frontRight, 1e-9), + () -> assertEquals(5.0, atMidpoint.rearLeft, 1e-9), + () -> assertEquals(6.0, atMidpoint.rearRight, 1e-9)); + + // Test interpolation at t=0.25 + final var atQuarter = start.interpolate(end, 0.25); + assertAll( + () -> assertEquals(2.0, atQuarter.frontLeft, 1e-9), + () -> assertEquals(3.0, atQuarter.frontRight, 1e-9), + () -> assertEquals(4.0, atQuarter.rearLeft, 1e-9), + () -> assertEquals(5.0, atQuarter.rearRight, 1e-9)); + + // Test clamping: t < 0 should clamp to 0 + final var belowRange = start.interpolate(end, -0.5); + assertAll( + () -> assertEquals(1.0, belowRange.frontLeft, 1e-9), + () -> assertEquals(2.0, belowRange.frontRight, 1e-9), + () -> assertEquals(3.0, belowRange.rearLeft, 1e-9), + () -> assertEquals(4.0, belowRange.rearRight, 1e-9)); + + // Test clamping: t > 1 should clamp to 1 + final var aboveRange = start.interpolate(end, 1.5); + assertAll( + () -> assertEquals(5.0, aboveRange.frontLeft, 1e-9), + () -> assertEquals(6.0, aboveRange.frontRight, 1e-9), + () -> assertEquals(7.0, aboveRange.rearLeft, 1e-9), + () -> assertEquals(8.0, aboveRange.rearRight, 1e-9)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index ad24ca616cc..e4fb5f2c5bd 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -415,7 +415,8 @@ void testTurnInPlaceInverseAccelerations() { /* The circumference of the wheels about the COR is π * diameter, or 2π * radius the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels - trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second squared, + trace out is 106.629190516in. + since we want our robot to rotate at 1 rotation per second squared, our wheels must trace out 1 rotation (or 106.63 inches) per second squared. */ @@ -444,7 +445,8 @@ void testTurnInPlaceForwardAccelerations() { Rotation2d.fromDegrees(-135), Rotation2d.fromDegrees(-45)); - var chassisAccelerations = m_kinematics.toChassisAccelerations(flAccel, frAccel, blAccel, brAccel); + var chassisAccelerations = + m_kinematics.toChassisAccelerations(flAccel, frAccel, blAccel, brAccel); assertAll( () -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon), diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java index 9e7c9e0cc88..3abb67b87d7 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java @@ -8,7 +8,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; @@ -51,8 +51,8 @@ void testEquals() { var moduleAccelerations2 = new SwerveModuleAccelerations(2.0, 1.5); var moduleAccelerations3 = new SwerveModuleAccelerations(2.1, 1.5); - assertTrue(moduleAccelerations1.equals(moduleAccelerations2)); - assertFalse(moduleAccelerations1.equals(moduleAccelerations3)); + assertEquals(moduleAccelerations1, moduleAccelerations2); + assertNotEquals(moduleAccelerations1, moduleAccelerations3); } @Test @@ -115,4 +115,46 @@ void testDivision() { () -> assertEquals(2.0, moduleAccelerations.acceleration), () -> assertEquals(1.0, moduleAccelerations.angularAcceleration)); } + + @Test + void testInterpolate() { + final var start = new SwerveModuleAccelerations(1.0, 2.0); + final var end = new SwerveModuleAccelerations(5.0, 6.0); + + // Test interpolation at t=0 (should return start) + final var atStart = start.interpolate(end, 0.0); + assertAll( + () -> assertEquals(1.0, atStart.acceleration, kEpsilon), + () -> assertEquals(2.0, atStart.angularAcceleration, kEpsilon)); + + // Test interpolation at t=1 (should return end) + final var atEnd = start.interpolate(end, 1.0); + assertAll( + () -> assertEquals(5.0, atEnd.acceleration, kEpsilon), + () -> assertEquals(6.0, atEnd.angularAcceleration, kEpsilon)); + + // Test interpolation at t=0.5 (should return midpoint) + final var atMidpoint = start.interpolate(end, 0.5); + assertAll( + () -> assertEquals(3.0, atMidpoint.acceleration, kEpsilon), + () -> assertEquals(4.0, atMidpoint.angularAcceleration, kEpsilon)); + + // Test interpolation at t=0.25 + final var atQuarter = start.interpolate(end, 0.25); + assertAll( + () -> assertEquals(2.0, atQuarter.acceleration, kEpsilon), + () -> assertEquals(3.0, atQuarter.angularAcceleration, kEpsilon)); + + // Test clamping: t < 0 should clamp to 0 + final var belowRange = start.interpolate(end, -0.5); + assertAll( + () -> assertEquals(1.0, belowRange.acceleration, kEpsilon), + () -> assertEquals(2.0, belowRange.angularAcceleration, kEpsilon)); + + // Test clamping: t > 1 should clamp to 1 + final var aboveRange = start.interpolate(end, 1.5); + assertAll( + () -> assertEquals(5.0, aboveRange.acceleration, kEpsilon), + () -> assertEquals(6.0, aboveRange.angularAcceleration, kEpsilon)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java index fdccdccb08b..f3c51555c0f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java @@ -149,4 +149,55 @@ void testCosineScale() { () -> assertEquals(Math.sqrt(2.0), refL.speed, kEpsilon), () -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon)); } + + @Test + void testInterpolate() { + // Test basic interpolation with simple angles + final var start = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0.0)); + final var end = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0)); + + // Test interpolation at t=0 (should return start) + final var atStart = start.interpolate(end, 0.0); + assertAll( + () -> assertEquals(1.0, atStart.speed, kEpsilon), + () -> assertEquals(0.0, atStart.angle.getDegrees(), kEpsilon)); + + // Test interpolation at t=1 (should return end) + final var atEnd = start.interpolate(end, 1.0); + assertAll( + () -> assertEquals(5.0, atEnd.speed, kEpsilon), + () -> assertEquals(90.0, atEnd.angle.getDegrees(), kEpsilon)); + + // Test interpolation at t=0.5 (should return midpoint) + final var atMidpoint = start.interpolate(end, 0.5); + assertAll( + () -> assertEquals(3.0, atMidpoint.speed, kEpsilon), + () -> assertEquals(45.0, atMidpoint.angle.getDegrees(), kEpsilon)); + + // Test interpolation at t=0.25 + final var atQuarter = start.interpolate(end, 0.25); + assertAll( + () -> assertEquals(2.0, atQuarter.speed, kEpsilon), + () -> assertEquals(22.5, atQuarter.angle.getDegrees(), kEpsilon)); + + // Test clamping: t < 0 should clamp to 0 + final var belowRange = start.interpolate(end, -0.5); + assertAll( + () -> assertEquals(1.0, belowRange.speed, kEpsilon), + () -> assertEquals(0.0, belowRange.angle.getDegrees(), kEpsilon)); + + // Test clamping: t > 1 should clamp to 1 + final var aboveRange = start.interpolate(end, 1.5); + assertAll( + () -> assertEquals(5.0, aboveRange.speed, kEpsilon), + () -> assertEquals(90.0, aboveRange.angle.getDegrees(), kEpsilon)); + + // Test angle wrapping with crossing 180/-180 boundary + final var startWrap = new SwerveModuleState(2.0, Rotation2d.fromDegrees(170.0)); + final var endWrap = new SwerveModuleState(4.0, Rotation2d.fromDegrees(-170.0)); + final var midpointWrap = startWrap.interpolate(endWrap, 0.5); + assertAll( + () -> assertEquals(3.0, midpointWrap.speed, kEpsilon), + () -> assertEquals(180.0, Math.abs(midpointWrap.angle.getDegrees()), kEpsilon)); + } } From 44b3f1301fb775ee07ebd774390f7bc524f5f3aa Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 21:57:21 -0400 Subject: [PATCH 16/82] fix: update documentation in ChassisAccelerations class to clarify acceleration vs velocity Signed-off-by: Zach Harel --- .../math/kinematics/ChassisAccelerations.java | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java index 33ccf7a69ac..fb86debe5bc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java @@ -21,9 +21,8 @@ /** * Represents the acceleration of a robot chassis. Although this class contains similar members - * compared to a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a - * change in pose w.r.t to the robot frame of reference, a ChassisAccelerations object represents a - * robot's velocity. + * compared to a ChassisSpeeds, they do NOT represent the same thing. Whereas a ChassisSpeeds object + * represents a robot's velocity, a ChassisAccelerations object represents a robot's acceleration. * *

A strictly non-holonomic drivetrain, such as a differential drive, should never have an ay * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum @@ -31,13 +30,13 @@ */ public class ChassisAccelerations implements ProtobufSerializable, StructSerializable, Interpolatable { - /** Velocity along the x-axis in meters per second². (Fwd is +) */ + /** Acceleration along the x-axis in meters per second squared. (Fwd is +) */ public double ax; - /** Velocity along the y-axis in meters per second². (Left is +) */ + /** Acceleration along the y-axis in meters per second squared. (Left is +) */ public double ay; - /** Angular velocity of the robot frame in radians per second². (CCW is +) */ + /** Angular acceleration of the robot frame in radians per second squared. (CCW is +) */ public double alpha; /** ChassisAccelerations struct for serialization. */ @@ -52,9 +51,9 @@ public ChassisAccelerations() {} /** * Constructs a ChassisAccelerations object. * - * @param ax Forward acceleration in meters per second². - * @param ay Sideways acceleration in meters per second². - * @param alpha Angular acceleration in radians per second. + * @param ax Forward acceleration in meters per second squared. + * @param ay Sideways acceleration in meters per second squared. + * @param alpha Angular acceleration in radians per second squared. */ public ChassisAccelerations(double ax, double ay, double alpha) { this.ax = ax; @@ -78,12 +77,14 @@ public ChassisAccelerations( } /** - * Converts this field-relative set of speeds into a robot-relative ChassisAccelerations object. + * Converts this field-relative set of accelerations into a robot-relative ChassisAccelerations + * object. * * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. - * @return ChassisAccelerations object representing the speeds in the robot's frame of reference. + * @return ChassisAccelerations object representing the accelerations in the robot's frame of + * reference. */ public ChassisAccelerations toRobotRelative(Rotation2d robotAngle) { // CW rotation into chassis frame @@ -92,12 +93,14 @@ public ChassisAccelerations toRobotRelative(Rotation2d robotAngle) { } /** - * Converts this robot-relative set of speeds into a field-relative ChassisAccelerations object. + * Converts this robot-relative set of accelerations into a field-relative ChassisAccelerations + * object. * * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. - * @return ChassisAccelerations object representing the speeds in the field's frame of reference. + * @return ChassisAccelerations object representing the accelerations in the field's frame of + * reference. */ public ChassisAccelerations toFieldRelative(Rotation2d robotAngle) { // CCW rotation out of chassis frame From a4e392e39d2145d3fa8c7d6fd8c9bc1829e1c8bc Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 22:09:02 -0400 Subject: [PATCH 17/82] refactor: replace angularAcceleration with angle in SwerveModuleAccelerations and related files Signed-off-by: Zach Harel --- .../edu/wpi/first/math/proto/Kinematics.java | 149 +++--- .../cpp/wpimath/protobuf/kinematics.npb.cpp | 497 +++++++++--------- .../cpp/wpimath/protobuf/kinematics.npb.h | 6 +- .../kinematics/SwerveModuleAccelerations.java | 92 +--- .../proto/SwerveModuleAccelerationsProto.java | 5 +- .../SwerveModuleAccelerationsStruct.java | 9 +- .../proto/SwerveModuleAccelerationsProto.cpp | 4 +- .../SwerveModuleAccelerationsStruct.cpp | 10 +- .../kinematics/SwerveModuleAccelerations.h | 41 +- .../struct/SwerveModuleAccelerationsStruct.h | 2 +- wpimath/src/main/proto/kinematics.proto | 2 +- 11 files changed, 384 insertions(+), 433 deletions(-) diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java index a272a1380b6..cd093333e70 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java @@ -19,7 +19,7 @@ import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4187, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4158, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + "BW9tZWdhIlQKHFByb3RvYnVmQ2hhc3Npc0FjY2VsZXJhdGlvbnMSDgoCYXgYASABKAFSAmF4Eg4KAmF5" + @@ -47,49 +47,49 @@ public final class Kinematics { "ZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5w" + "cm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUS" + "FAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90" + - "YXRpb24yZFIFYW5nbGUiegohUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb25zEiIKDGFjY2Vs" + - "ZXJhdGlvbhgBIAEoAVIMYWNjZWxlcmF0aW9uEjEKFGFuZ3VsYXJfYWNjZWxlcmF0aW9uGAIgASgBUhNh" + - "bmd1bGFyQWNjZWxlcmF0aW9uQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0rWEgoGEgQAAFQBCggK" + - "AQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGADEKCQoCCAESAwYAMQoKCgIEABIECAAM" + - "AQoKCgMEAAESAwgIHQoLCgQEAAIAEgMJAhAKDAoFBAACAAUSAwkCCAoMCgUEAAIAARIDCQkLCgwKBQQA" + - "AgADEgMJDg8KCwoEBAACARIDCgIQCgwKBQQAAgEFEgMKAggKDAoFBAACAQESAwoJCwoMCgUEAAIBAxID" + - "Cg4PCgsKBAQAAgISAwsCEwoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCQ4KDAoFBAACAgMSAwsREgoK" + - "CgIEARIEDgASAQoKCgMEAQESAw4IJAoLCgQEAQIAEgMPAhAKDAoFBAECAAUSAw8CCAoMCgUEAQIAARID" + - "DwkLCgwKBQQBAgADEgMPDg8KCwoEBAECARIDEAIQCgwKBQQBAgEFEgMQAggKDAoFBAECAQESAxAJCwoM" + - "CgUEAQIBAxIDEA4PCgsKBAQBAgISAxECEwoMCgUEAQICBRIDEQIICgwKBQQBAgIBEgMRCQ4KDAoFBAEC" + - "AgMSAxEREgoKCgIEAhIEFAAWAQoKCgMEAgESAxQIKwoLCgQEAgIAEgMVAhgKDAoFBAICAAUSAxUCCAoM" + - "CgUEAgIAARIDFQkTCgwKBQQCAgADEgMVFhcKCgoCBAMSBBgAGwEKCgoDBAMBEgMYCCwKCwoEBAMCABID" + - "GQISCgwKBQQDAgAFEgMZAggKDAoFBAMCAAESAxkJDQoMCgUEAwIAAxIDGRARCgsKBAQDAgESAxoCEwoM", - "CgUEAwIBBRIDGgIICgwKBQQDAgEBEgMaCQ4KDAoFBAMCAQMSAxoREgoKCgIEBBIEHQAgAQoKCgMEBAES" + - "Ax0IMwoLCgQEBAIAEgMeAhIKDAoFBAQCAAUSAx4CCAoMCgUEBAIAARIDHgkNCgwKBQQEAgADEgMeEBEK" + - "CwoEBAQCARIDHwITCgwKBQQEAgEFEgMfAggKDAoFBAQCAQESAx8JDgoMCgUEBAIBAxIDHxESCgoKAgQF" + - "EgQiACUBCgoKAwQFARIDIggvCgsKBAQFAgASAyMCEgoMCgUEBQIABRIDIwIICgwKBQQFAgABEgMjCQ0K" + - "DAoFBAUCAAMSAyMQEQoLCgQEBQIBEgMkAhMKDAoFBAUCAQUSAyQCCAoMCgUEBQIBARIDJAkOCgwKBQQF" + - "AgEDEgMkERIKCgoCBAYSBCcALAEKCgoDBAYBEgMnCCYKCwoEBAYCABIDKAInCgwKBQQGAgAGEgMoAhcK" + - "DAoFBAYCAAESAygYIgoMCgUEBgIAAxIDKCUmCgsKBAQGAgESAykCKAoMCgUEBgIBBhIDKQIXCgwKBQQG" + - "AgEBEgMpGCMKDAoFBAYCAQMSAykmJwoLCgQEBgICEgMqAiYKDAoFBAYCAgYSAyoCFwoMCgUEBgICARID" + - "KhghCgwKBQQGAgIDEgMqJCUKCwoEBAYCAxIDKwInCgwKBQQGAgMGEgMrAhcKDAoFBAYCAwESAysYIgoM" + - "CgUEBgIDAxIDKyUmCgoKAgQHEgQuADMBCgoKAwQHARIDLggqCgsKBAQHAgASAy8CGAoMCgUEBwIABRID" + - "LwIICgwKBQQHAgABEgMvCRMKDAoFBAcCAAMSAy8WFwoLCgQEBwIBEgMwAhkKDAoFBAcCAQUSAzACCAoM" + - "CgUEBwIBARIDMAkUCgwKBQQHAgEDEgMwFxgKCwoEBAcCAhIDMQIXCgwKBQQHAgIFEgMxAggKDAoFBAcC" + - "AgESAzEJEgoMCgUEBwICAxIDMRUWCgsKBAQHAgMSAzICGAoMCgUEBwIDBRIDMgIICgwKBQQHAgMBEgMy" + - "CRMKDAoFBAcCAwMSAzIWFwoKCgIECBIENQA6AQoKCgMECAESAzUIJwoLCgQECAIAEgM2AhgKDAoFBAgC" + - "AAUSAzYCCAoMCgUECAIAARIDNgkTCgwKBQQIAgADEgM2FhcKCwoEBAgCARIDNwIZCgwKBQQIAgEFEgM3" + - "AggKDAoFBAgCAQESAzcJFAoMCgUECAIBAxIDNxcYCgsKBAQIAgISAzgCFwoMCgUECAICBRIDOAIICgwK" + - "BQQIAgIBEgM4CRIKDAoFBAgCAgMSAzgVFgoLCgQECAIDEgM5AhgKDAoFBAgCAwUSAzkCCAoMCgUECAID" + - "ARIDOQkTCgwKBQQIAgMDEgM5FhcKCgoCBAkSBDwAQQEKCgoDBAkBEgM8CC4KCwoEBAkCABIDPQIYCgwK" + - "BQQJAgAFEgM9AggKDAoFBAkCAAESAz0JEwoMCgUECQIAAxIDPRYXCgsKBAQJAgESAz4CGQoMCgUECQIB" + - "BRIDPgIICgwKBQQJAgEBEgM+CRQKDAoFBAkCAQMSAz4XGAoLCgQECQICEgM/AhcKDAoFBAkCAgUSAz8C", - "CAoMCgUECQICARIDPwkSCgwKBQQJAgIDEgM/FRYKCwoEBAkCAxIDQAIYCgwKBQQJAgMFEgNAAggKDAoF" + - "BAkCAwESA0AJEwoMCgUECQIDAxIDQBYXCgoKAgQKEgRDAEUBCgoKAwQKARIDQwglCgsKBAQKAgASA0QC" + - "LQoMCgUECgIABBIDRAIKCgwKBQQKAgAGEgNECyAKDAoFBAoCAAESA0QhKAoMCgUECgIAAxIDRCssCgoK" + - "AgQLEgRHAEoBCgoKAwQLARIDRwgkCgsKBAQLAgASA0gCFgoMCgUECwIABRIDSAIICgwKBQQLAgABEgNI" + - "CREKDAoFBAsCAAMSA0gUFQoLCgQECwIBEgNJAh8KDAoFBAsCAQYSA0kCFAoMCgUECwIBARIDSRUaCgwK" + - "BQQLAgEDEgNJHR4KCgoCBAwSBEwATwEKCgoDBAwBEgNMCCEKCwoEBAwCABIDTQITCgwKBQQMAgAFEgNN" + - "AggKDAoFBAwCAAESA00JDgoMCgUEDAIAAxIDTRESCgsKBAQMAgESA04CHwoMCgUEDAIBBhIDTgIUCgwK" + - "BQQMAgEBEgNOFRoKDAoFBAwCAQMSA04dHgoKCgIEDRIEUQBUAQoKCgMEDQESA1EIKQoLCgQEDQIAEgNS" + - "AhoKDAoFBA0CAAUSA1ICCAoMCgUEDQIAARIDUgkVCgwKBQQNAgADEgNSGBkKCwoEBA0CARIDUwIiCgwK" + - "BQQNAgEFEgNTAggKDAoFBA0CAQESA1MJHQoMCgUEDQIBAxIDUyAhYgZwcm90bzM="); + "YXRpb24yZFIFYW5nbGUiXQohUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb25zEiIKDGFjY2Vs" + + "ZXJhdGlvbhgBIAEoAVIMYWNjZWxlcmF0aW9uEhQKBWFuZ2xlGAIgASgBUgVhbmdsZUIaChhlZHUud3Bp" + + "LmZpcnN0Lm1hdGgucHJvdG9K1hIKBhIEAABUAQoICgEMEgMAABIKCAoBAhIDAgASCgkKAgMAEgMEABoK" + + "CAoBCBIDBgAxCgkKAggBEgMGADEKCgoCBAASBAgADAEKCgoDBAABEgMICB0KCwoEBAACABIDCQIQCgwK" + + "BQQAAgAFEgMJAggKDAoFBAACAAESAwkJCwoMCgUEAAIAAxIDCQ4PCgsKBAQAAgESAwoCEAoMCgUEAAIB" + + "BRIDCgIICgwKBQQAAgEBEgMKCQsKDAoFBAACAQMSAwoODwoLCgQEAAICEgMLAhMKDAoFBAACAgUSAwsC" + + "CAoMCgUEAAICARIDCwkOCgwKBQQAAgIDEgMLERIKCgoCBAESBA4AEgEKCgoDBAEBEgMOCCQKCwoEBAEC" + + "ABIDDwIQCgwKBQQBAgAFEgMPAggKDAoFBAECAAESAw8JCwoMCgUEAQIAAxIDDw4PCgsKBAQBAgESAxAC" + + "EAoMCgUEAQIBBRIDEAIICgwKBQQBAgEBEgMQCQsKDAoFBAECAQMSAxAODwoLCgQEAQICEgMRAhMKDAoF" + + "BAECAgUSAxECCAoMCgUEAQICARIDEQkOCgwKBQQBAgIDEgMRERIKCgoCBAISBBQAFgEKCgoDBAIBEgMU" + + "CCsKCwoEBAICABIDFQIYCgwKBQQCAgAFEgMVAggKDAoFBAICAAESAxUJEwoMCgUEAgIAAxIDFRYXCgoK" + + "AgQDEgQYABsBCgoKAwQDARIDGAgsCgsKBAQDAgASAxkCEgoMCgUEAwIABRIDGQIICgwKBQQDAgABEgMZ" + + "CQ0KDAoFBAMCAAMSAxkQEQoLCgQEAwIBEgMaAhMKDAoFBAMCAQUSAxoCCAoMCgUEAwIBARIDGgkOCgwK", + "BQQDAgEDEgMaERIKCgoCBAQSBB0AIAEKCgoDBAQBEgMdCDMKCwoEBAQCABIDHgISCgwKBQQEAgAFEgMe" + + "AggKDAoFBAQCAAESAx4JDQoMCgUEBAIAAxIDHhARCgsKBAQEAgESAx8CEwoMCgUEBAIBBRIDHwIICgwK" + + "BQQEAgEBEgMfCQ4KDAoFBAQCAQMSAx8REgoKCgIEBRIEIgAlAQoKCgMEBQESAyIILwoLCgQEBQIAEgMj" + + "AhIKDAoFBAUCAAUSAyMCCAoMCgUEBQIAARIDIwkNCgwKBQQFAgADEgMjEBEKCwoEBAUCARIDJAITCgwK" + + "BQQFAgEFEgMkAggKDAoFBAUCAQESAyQJDgoMCgUEBQIBAxIDJBESCgoKAgQGEgQnACwBCgoKAwQGARID" + + "JwgmCgsKBAQGAgASAygCJwoMCgUEBgIABhIDKAIXCgwKBQQGAgABEgMoGCIKDAoFBAYCAAMSAyglJgoL" + + "CgQEBgIBEgMpAigKDAoFBAYCAQYSAykCFwoMCgUEBgIBARIDKRgjCgwKBQQGAgEDEgMpJicKCwoEBAYC" + + "AhIDKgImCgwKBQQGAgIGEgMqAhcKDAoFBAYCAgESAyoYIQoMCgUEBgICAxIDKiQlCgsKBAQGAgMSAysC" + + "JwoMCgUEBgIDBhIDKwIXCgwKBQQGAgMBEgMrGCIKDAoFBAYCAwMSAyslJgoKCgIEBxIELgAzAQoKCgME" + + "BwESAy4IKgoLCgQEBwIAEgMvAhgKDAoFBAcCAAUSAy8CCAoMCgUEBwIAARIDLwkTCgwKBQQHAgADEgMv" + + "FhcKCwoEBAcCARIDMAIZCgwKBQQHAgEFEgMwAggKDAoFBAcCAQESAzAJFAoMCgUEBwIBAxIDMBcYCgsK" + + "BAQHAgISAzECFwoMCgUEBwICBRIDMQIICgwKBQQHAgIBEgMxCRIKDAoFBAcCAgMSAzEVFgoLCgQEBwID" + + "EgMyAhgKDAoFBAcCAwUSAzICCAoMCgUEBwIDARIDMgkTCgwKBQQHAgMDEgMyFhcKCgoCBAgSBDUAOgEK" + + "CgoDBAgBEgM1CCcKCwoEBAgCABIDNgIYCgwKBQQIAgAFEgM2AggKDAoFBAgCAAESAzYJEwoMCgUECAIA" + + "AxIDNhYXCgsKBAQIAgESAzcCGQoMCgUECAIBBRIDNwIICgwKBQQIAgEBEgM3CRQKDAoFBAgCAQMSAzcX" + + "GAoLCgQECAICEgM4AhcKDAoFBAgCAgUSAzgCCAoMCgUECAICARIDOAkSCgwKBQQIAgIDEgM4FRYKCwoE" + + "BAgCAxIDOQIYCgwKBQQIAgMFEgM5AggKDAoFBAgCAwESAzkJEwoMCgUECAIDAxIDORYXCgoKAgQJEgQ8" + + "AEEBCgoKAwQJARIDPAguCgsKBAQJAgASAz0CGAoMCgUECQIABRIDPQIICgwKBQQJAgABEgM9CRMKDAoF" + + "BAkCAAMSAz0WFwoLCgQECQIBEgM+AhkKDAoFBAkCAQUSAz4CCAoMCgUECQIBARIDPgkUCgwKBQQJAgED" + + "EgM+FxgKCwoEBAkCAhIDPwIXCgwKBQQJAgIFEgM/AggKDAoFBAkCAgESAz8JEgoMCgUECQICAxIDPxUW", + "CgsKBAQJAgMSA0ACGAoMCgUECQIDBRIDQAIICgwKBQQJAgMBEgNACRMKDAoFBAkCAwMSA0AWFwoKCgIE" + + "ChIEQwBFAQoKCgMECgESA0MIJQoLCgQECgIAEgNEAi0KDAoFBAoCAAQSA0QCCgoMCgUECgIABhIDRAsg" + + "CgwKBQQKAgABEgNEISgKDAoFBAoCAAMSA0QrLAoKCgIECxIERwBKAQoKCgMECwESA0cIJAoLCgQECwIA" + + "EgNIAhYKDAoFBAsCAAUSA0gCCAoMCgUECwIAARIDSAkRCgwKBQQLAgADEgNIFBUKCwoEBAsCARIDSQIf" + + "CgwKBQQLAgEGEgNJAhQKDAoFBAsCAQESA0kVGgoMCgUECwIBAxIDSR0eCgoKAgQMEgRMAE8BCgoKAwQM" + + "ARIDTAghCgsKBAQMAgASA00CEwoMCgUEDAIABRIDTQIICgwKBQQMAgABEgNNCQ4KDAoFBAwCAAMSA00R" + + "EgoLCgQEDAIBEgNOAh8KDAoFBAwCAQYSA04CFAoMCgUEDAIBARIDThUaCgwKBQQMAgEDEgNOHR4KCgoC" + + "BA0SBFEAVAEKCgoDBA0BEgNRCCkKCwoEBA0CABIDUgIaCgwKBQQNAgAFEgNSAggKDAoFBA0CAAESA1IJ" + + "FQoMCgUEDQIAAxIDUhgZCgsKBAQNAgESA1MCEwoMCgUEDQIBBRIDUwIICgwKBQQNAgEBEgNTCQ4KDAoF" + + "BA0CAQMSA1MREmIGcHJvdG8z"); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); @@ -119,7 +119,7 @@ public final class Kinematics { static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1532, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleAccelerations_descriptor = descriptor.internalContainedType(1636, 122, "ProtobufSwerveModuleAccelerations", "wpi.proto.ProtobufSwerveModuleAccelerations"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleAccelerations_descriptor = descriptor.internalContainedType(1636, 93, "ProtobufSwerveModuleAccelerations", "wpi.proto.ProtobufSwerveModuleAccelerations"); /** * @return this proto file's descriptor. @@ -5321,9 +5321,9 @@ public static final class ProtobufSwerveModuleAccelerations extends ProtoMessage private double acceleration; /** - * optional double angular_acceleration = 2; + * optional double angle = 2; */ - private double angularAcceleration; + private double angle; private ProtobufSwerveModuleAccelerations() { } @@ -5373,39 +5373,39 @@ public ProtobufSwerveModuleAccelerations setAcceleration(final double value) { } /** - * optional double angular_acceleration = 2; - * @return whether the angularAcceleration field is set + * optional double angle = 2; + * @return whether the angle field is set */ - public boolean hasAngularAcceleration() { + public boolean hasAngle() { return (bitField0_ & 0x00000002) != 0; } /** - * optional double angular_acceleration = 2; + * optional double angle = 2; * @return this */ - public ProtobufSwerveModuleAccelerations clearAngularAcceleration() { + public ProtobufSwerveModuleAccelerations clearAngle() { bitField0_ &= ~0x00000002; - angularAcceleration = 0D; + angle = 0D; return this; } /** - * optional double angular_acceleration = 2; - * @return the angularAcceleration + * optional double angle = 2; + * @return the angle */ - public double getAngularAcceleration() { - return angularAcceleration; + public double getAngle() { + return angle; } /** - * optional double angular_acceleration = 2; - * @param value the angularAcceleration to set + * optional double angle = 2; + * @param value the angle to set * @return this */ - public ProtobufSwerveModuleAccelerations setAngularAcceleration(final double value) { + public ProtobufSwerveModuleAccelerations setAngle(final double value) { bitField0_ |= 0x00000002; - angularAcceleration = value; + angle = value; return this; } @@ -5416,7 +5416,7 @@ public ProtobufSwerveModuleAccelerations copyFrom( if ((bitField0_ | other.bitField0_) != 0) { bitField0_ = other.bitField0_; acceleration = other.acceleration; - angularAcceleration = other.angularAcceleration; + angle = other.angle; } return this; } @@ -5431,8 +5431,8 @@ public ProtobufSwerveModuleAccelerations mergeFrom( if (other.hasAcceleration()) { setAcceleration(other.acceleration); } - if (other.hasAngularAcceleration()) { - setAngularAcceleration(other.angularAcceleration); + if (other.hasAngle()) { + setAngle(other.angle); } return this; } @@ -5445,7 +5445,7 @@ public ProtobufSwerveModuleAccelerations clear() { cachedSize = -1; bitField0_ = 0; acceleration = 0D; - angularAcceleration = 0D; + angle = 0D; return this; } @@ -5470,7 +5470,7 @@ public boolean equals(Object o) { ProtobufSwerveModuleAccelerations other = (ProtobufSwerveModuleAccelerations) o; return bitField0_ == other.bitField0_ && (!hasAcceleration() || ProtoUtil.isEqual(acceleration, other.acceleration)) - && (!hasAngularAcceleration() || ProtoUtil.isEqual(angularAcceleration, other.angularAcceleration)); + && (!hasAngle() || ProtoUtil.isEqual(angle, other.angle)); } @Override @@ -5481,7 +5481,7 @@ public void writeTo(final ProtoSink output) throws IOException { } if ((bitField0_ & 0x00000002) != 0) { output.writeRawByte((byte) 17); - output.writeDoubleNoTag(angularAcceleration); + output.writeDoubleNoTag(angle); } } @@ -5514,8 +5514,8 @@ public ProtobufSwerveModuleAccelerations mergeFrom(final ProtoSource input) thro } } case 17: { - // angularAcceleration - angularAcceleration = input.readDouble(); + // angle + angle = input.readDouble(); bitField0_ |= 0x00000002; tag = input.readTag(); if (tag != 0) { @@ -5543,7 +5543,7 @@ public void writeTo(final JsonSink output) throws IOException { output.writeDouble(FieldNames.acceleration, acceleration); } if ((bitField0_ & 0x00000002) != 0) { - output.writeDouble(FieldNames.angularAcceleration, angularAcceleration); + output.writeDouble(FieldNames.angle, angle); } output.endObject(); } @@ -5566,11 +5566,10 @@ public ProtobufSwerveModuleAccelerations mergeFrom(final JsonSource input) throw } break; } - case -1643072638: - case 1425936317: { - if (input.isAtField(FieldNames.angularAcceleration)) { + case 92960979: { + if (input.isAtField(FieldNames.angle)) { if (!input.trySkipNullValue()) { - angularAcceleration = input.readDouble(); + angle = input.readDouble(); bitField0_ |= 0x00000002; } } else { @@ -5642,7 +5641,7 @@ public ProtobufSwerveModuleAccelerations create() { static class FieldNames { static final FieldName acceleration = FieldName.forField("acceleration"); - static final FieldName angularAcceleration = FieldName.forField("angularAcceleration", "angular_acceleration"); + static final FieldName angle = FieldName.forField("angle"); } } } diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp index b4ac8539c6f..57e60de84a2 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp @@ -175,262 +175,259 @@ static const uint8_t file_descriptor[] { 0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50, 0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74, 0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61, -0x6e,0x67,0x6c,0x65,0x22,0x7a,0x0a,0x21,0x50,0x72, +0x6e,0x67,0x6c,0x65,0x22,0x5d,0x0a,0x21,0x50,0x72, 0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72, 0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65,0x41,0x63, 0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,0x6e, 0x73,0x12,0x22,0x0a,0x0c,0x61,0x63,0x63,0x65,0x6c, 0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x18,0x01,0x20, 0x01,0x28,0x01,0x52,0x0c,0x61,0x63,0x63,0x65,0x6c, -0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x12,0x31,0x0a, -0x14,0x61,0x6e,0x67,0x75,0x6c,0x61,0x72,0x5f,0x61, -0x63,0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f, -0x6e,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x13,0x61, -0x6e,0x67,0x75,0x6c,0x61,0x72,0x41,0x63,0x63,0x65, -0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x42,0x1a, -0x0a,0x18,0x65,0x64,0x75,0x2e,0x77,0x70,0x69,0x2e, -0x66,0x69,0x72,0x73,0x74,0x2e,0x6d,0x61,0x74,0x68, -0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0xd6,0x12,0x0a, -0x06,0x12,0x04,0x00,0x00,0x54,0x01,0x0a,0x08,0x0a, -0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08,0x0a, -0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x09,0x0a, -0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x1a,0x0a,0x08, -0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x31,0x0a,0x09, -0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00,0x31,0x0a, -0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x08,0x00,0x0c, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12,0x03, -0x08,0x08,0x1d,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02, -0x00,0x12,0x03,0x09,0x02,0x10,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x09,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01,0x12, -0x03,0x09,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x00,0x03,0x12,0x03,0x09,0x0e,0x0f,0x0a,0x0b, -0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x0a,0x02, -0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x05, -0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x01,0x01,0x12,0x03,0x0a,0x09,0x0b,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12,0x03, -0x0a,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02, -0x02,0x12,0x03,0x0b,0x02,0x13,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x0b,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01,0x12, -0x03,0x0b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x02,0x03,0x12,0x03,0x0b,0x11,0x12,0x0a,0x0a, -0x0a,0x02,0x04,0x01,0x12,0x04,0x0e,0x00,0x12,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,0x03,0x0e, -0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x00, -0x12,0x03,0x0f,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04, -0x01,0x02,0x00,0x05,0x12,0x03,0x0f,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x01,0x12,0x03, -0x0f,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, -0x00,0x03,0x12,0x03,0x0f,0x0e,0x0f,0x0a,0x0b,0x0a, -0x04,0x04,0x01,0x02,0x01,0x12,0x03,0x10,0x02,0x10, -0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x05,0x12, -0x03,0x10,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01, -0x02,0x01,0x01,0x12,0x03,0x10,0x09,0x0b,0x0a,0x0c, -0x0a,0x05,0x04,0x01,0x02,0x01,0x03,0x12,0x03,0x10, -0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x02, -0x12,0x03,0x11,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04, -0x01,0x02,0x02,0x05,0x12,0x03,0x11,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x01,0x12,0x03, -0x11,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, -0x02,0x03,0x12,0x03,0x11,0x11,0x12,0x0a,0x0a,0x0a, -0x02,0x04,0x02,0x12,0x04,0x14,0x00,0x16,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x02,0x01,0x12,0x03,0x14,0x08, -0x2b,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x00,0x12, -0x03,0x15,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x02, -0x02,0x00,0x05,0x12,0x03,0x15,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x02,0x02,0x00,0x01,0x12,0x03,0x15, -0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00, -0x03,0x12,0x03,0x15,0x16,0x17,0x0a,0x0a,0x0a,0x02, -0x04,0x03,0x12,0x04,0x18,0x00,0x1b,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x18,0x08,0x2c, -0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12,0x03, -0x19,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, -0x00,0x05,0x12,0x03,0x19,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x19,0x09, -0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x03, -0x12,0x03,0x19,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04, -0x03,0x02,0x01,0x12,0x03,0x1a,0x02,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03,0x1a, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01, -0x01,0x12,0x03,0x1a,0x09,0x0e,0x0a,0x0c,0x0a,0x05, -0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x1a,0x11,0x12, -0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1d,0x00, -0x20,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01,0x12, -0x03,0x1d,0x08,0x33,0x0a,0x0b,0x0a,0x04,0x04,0x04, -0x02,0x00,0x12,0x03,0x1e,0x02,0x12,0x0a,0x0c,0x0a, -0x05,0x04,0x04,0x02,0x00,0x05,0x12,0x03,0x1e,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x01, -0x12,0x03,0x1e,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04, -0x04,0x02,0x00,0x03,0x12,0x03,0x1e,0x10,0x11,0x0a, -0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03,0x1f, -0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01, -0x05,0x12,0x03,0x1f,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x1f,0x09,0x0e, -0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03,0x12, -0x03,0x1f,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x05, -0x12,0x04,0x22,0x00,0x25,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x05,0x01,0x12,0x03,0x22,0x08,0x2f,0x0a,0x0b, -0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x23,0x02, -0x12,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x05, -0x12,0x03,0x23,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x05,0x02,0x00,0x01,0x12,0x03,0x23,0x09,0x0d,0x0a, -0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03, -0x23,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02, -0x01,0x12,0x03,0x24,0x02,0x13,0x0a,0x0c,0x0a,0x05, -0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x24,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01,0x12, -0x03,0x24,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x05, -0x02,0x01,0x03,0x12,0x03,0x24,0x11,0x12,0x0a,0x0a, -0x0a,0x02,0x04,0x06,0x12,0x04,0x27,0x00,0x2c,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03,0x27, -0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x00, -0x12,0x03,0x28,0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x00,0x06,0x12,0x03,0x28,0x02,0x17,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12,0x03, -0x28,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, -0x00,0x03,0x12,0x03,0x28,0x25,0x26,0x0a,0x0b,0x0a, -0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x29,0x02,0x28, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x06,0x12, -0x03,0x29,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x01,0x01,0x12,0x03,0x29,0x18,0x23,0x0a,0x0c, -0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x29, -0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x02, -0x12,0x03,0x2a,0x02,0x26,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x02,0x06,0x12,0x03,0x2a,0x02,0x17,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03, -0x2a,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, -0x02,0x03,0x12,0x03,0x2a,0x24,0x25,0x0a,0x0b,0x0a, -0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2b,0x02,0x27, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x06,0x12, -0x03,0x2b,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x03,0x01,0x12,0x03,0x2b,0x18,0x22,0x0a,0x0c, -0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03,0x2b, -0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12,0x04, -0x2e,0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x07, -0x01,0x12,0x03,0x2e,0x08,0x2a,0x0a,0x0b,0x0a,0x04, -0x04,0x07,0x02,0x00,0x12,0x03,0x2f,0x02,0x18,0x0a, -0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x05,0x12,0x03, -0x2f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, -0x00,0x01,0x12,0x03,0x2f,0x09,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x07,0x02,0x00,0x03,0x12,0x03,0x2f,0x16, -0x17,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x01,0x12, -0x03,0x30,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x07, -0x02,0x01,0x05,0x12,0x03,0x30,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x07,0x02,0x01,0x01,0x12,0x03,0x30, -0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01, -0x03,0x12,0x03,0x30,0x17,0x18,0x0a,0x0b,0x0a,0x04, -0x04,0x07,0x02,0x02,0x12,0x03,0x31,0x02,0x17,0x0a, -0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x05,0x12,0x03, -0x31,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, -0x02,0x01,0x12,0x03,0x31,0x09,0x12,0x0a,0x0c,0x0a, -0x05,0x04,0x07,0x02,0x02,0x03,0x12,0x03,0x31,0x15, -0x16,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x03,0x12, -0x03,0x32,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07, -0x02,0x03,0x05,0x12,0x03,0x32,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x07,0x02,0x03,0x01,0x12,0x03,0x32, -0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03, -0x03,0x12,0x03,0x32,0x16,0x17,0x0a,0x0a,0x0a,0x02, -0x04,0x08,0x12,0x04,0x35,0x00,0x3a,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x08,0x01,0x12,0x03,0x35,0x08,0x27, -0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x00,0x12,0x03, -0x36,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, -0x00,0x05,0x12,0x03,0x36,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x00,0x01,0x12,0x03,0x36,0x09, -0x13,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x03, -0x12,0x03,0x36,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04, -0x08,0x02,0x01,0x12,0x03,0x37,0x02,0x19,0x0a,0x0c, -0x0a,0x05,0x04,0x08,0x02,0x01,0x05,0x12,0x03,0x37, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01, -0x01,0x12,0x03,0x37,0x09,0x14,0x0a,0x0c,0x0a,0x05, -0x04,0x08,0x02,0x01,0x03,0x12,0x03,0x37,0x17,0x18, -0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x02,0x12,0x03, -0x38,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, -0x02,0x05,0x12,0x03,0x38,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x02,0x01,0x12,0x03,0x38,0x09, -0x12,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x03, -0x12,0x03,0x38,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04, -0x08,0x02,0x03,0x12,0x03,0x39,0x02,0x18,0x0a,0x0c, -0x0a,0x05,0x04,0x08,0x02,0x03,0x05,0x12,0x03,0x39, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03, -0x01,0x12,0x03,0x39,0x09,0x13,0x0a,0x0c,0x0a,0x05, -0x04,0x08,0x02,0x03,0x03,0x12,0x03,0x39,0x16,0x17, -0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,0x3c,0x00, -0x41,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01,0x12, -0x03,0x3c,0x08,0x2e,0x0a,0x0b,0x0a,0x04,0x04,0x09, -0x02,0x00,0x12,0x03,0x3d,0x02,0x18,0x0a,0x0c,0x0a, -0x05,0x04,0x09,0x02,0x00,0x05,0x12,0x03,0x3d,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x01, -0x12,0x03,0x3d,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04, -0x09,0x02,0x00,0x03,0x12,0x03,0x3d,0x16,0x17,0x0a, -0x0b,0x0a,0x04,0x04,0x09,0x02,0x01,0x12,0x03,0x3e, -0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01, -0x05,0x12,0x03,0x3e,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x01,0x01,0x12,0x03,0x3e,0x09,0x14, -0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x03,0x12, -0x03,0x3e,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x09, -0x02,0x02,0x12,0x03,0x3f,0x02,0x17,0x0a,0x0c,0x0a, -0x05,0x04,0x09,0x02,0x02,0x05,0x12,0x03,0x3f,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x01, -0x12,0x03,0x3f,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04, -0x09,0x02,0x02,0x03,0x12,0x03,0x3f,0x15,0x16,0x0a, -0x0b,0x0a,0x04,0x04,0x09,0x02,0x03,0x12,0x03,0x40, -0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03, -0x05,0x12,0x03,0x40,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x03,0x01,0x12,0x03,0x40,0x09,0x13, -0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x03,0x12, -0x03,0x40,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x0a, -0x12,0x04,0x43,0x00,0x45,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x0a,0x01,0x12,0x03,0x43,0x08,0x25,0x0a,0x0b, -0x0a,0x04,0x04,0x0a,0x02,0x00,0x12,0x03,0x44,0x02, -0x2d,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x04, -0x12,0x03,0x44,0x02,0x0a,0x0a,0x0c,0x0a,0x05,0x04, -0x0a,0x02,0x00,0x06,0x12,0x03,0x44,0x0b,0x20,0x0a, -0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x01,0x12,0x03, -0x44,0x21,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02, -0x00,0x03,0x12,0x03,0x44,0x2b,0x2c,0x0a,0x0a,0x0a, -0x02,0x04,0x0b,0x12,0x04,0x47,0x00,0x4a,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x0b,0x01,0x12,0x03,0x47,0x08, -0x24,0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02,0x00,0x12, -0x03,0x48,0x02,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x0b, -0x02,0x00,0x05,0x12,0x03,0x48,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x0b,0x02,0x00,0x01,0x12,0x03,0x48, -0x09,0x11,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00, -0x03,0x12,0x03,0x48,0x14,0x15,0x0a,0x0b,0x0a,0x04, -0x04,0x0b,0x02,0x01,0x12,0x03,0x49,0x02,0x1f,0x0a, -0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x06,0x12,0x03, -0x49,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02, -0x01,0x01,0x12,0x03,0x49,0x15,0x1a,0x0a,0x0c,0x0a, -0x05,0x04,0x0b,0x02,0x01,0x03,0x12,0x03,0x49,0x1d, -0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12,0x04,0x4c, -0x00,0x4f,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0c,0x01, -0x12,0x03,0x4c,0x08,0x21,0x0a,0x0b,0x0a,0x04,0x04, -0x0c,0x02,0x00,0x12,0x03,0x4d,0x02,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x0c,0x02,0x00,0x05,0x12,0x03,0x4d, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00, -0x01,0x12,0x03,0x4d,0x09,0x0e,0x0a,0x0c,0x0a,0x05, -0x04,0x0c,0x02,0x00,0x03,0x12,0x03,0x4d,0x11,0x12, -0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01,0x12,0x03, -0x4e,0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02, -0x01,0x06,0x12,0x03,0x4e,0x02,0x14,0x0a,0x0c,0x0a, -0x05,0x04,0x0c,0x02,0x01,0x01,0x12,0x03,0x4e,0x15, -0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x03, -0x12,0x03,0x4e,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04, -0x0d,0x12,0x04,0x51,0x00,0x54,0x01,0x0a,0x0a,0x0a, -0x03,0x04,0x0d,0x01,0x12,0x03,0x51,0x08,0x29,0x0a, -0x0b,0x0a,0x04,0x04,0x0d,0x02,0x00,0x12,0x03,0x52, -0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00, -0x05,0x12,0x03,0x52,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x0d,0x02,0x00,0x01,0x12,0x03,0x52,0x09,0x15, -0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x03,0x12, -0x03,0x52,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x0d, -0x02,0x01,0x12,0x03,0x53,0x02,0x22,0x0a,0x0c,0x0a, -0x05,0x04,0x0d,0x02,0x01,0x05,0x12,0x03,0x53,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x01, -0x12,0x03,0x53,0x09,0x1d,0x0a,0x0c,0x0a,0x05,0x04, -0x0d,0x02,0x01,0x03,0x12,0x03,0x53,0x20,0x21,0x62, -0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, +0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x12,0x14,0x0a, +0x05,0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01, +0x28,0x01,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65,0x42, +0x1a,0x0a,0x18,0x65,0x64,0x75,0x2e,0x77,0x70,0x69, +0x2e,0x66,0x69,0x72,0x73,0x74,0x2e,0x6d,0x61,0x74, +0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0xd6,0x12, +0x0a,0x06,0x12,0x04,0x00,0x00,0x54,0x01,0x0a,0x08, +0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08, +0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x09, +0x0a,0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x1a,0x0a, +0x08,0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x31,0x0a, +0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00,0x31, +0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x08,0x00, +0x0c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12, +0x03,0x08,0x08,0x1d,0x0a,0x0b,0x0a,0x04,0x04,0x00, +0x02,0x00,0x12,0x03,0x09,0x02,0x10,0x0a,0x0c,0x0a, +0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x09,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01, +0x12,0x03,0x09,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x00,0x03,0x12,0x03,0x09,0x0e,0x0f,0x0a, +0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x0a, +0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01, +0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0a,0x09,0x0b, +0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12, +0x03,0x0a,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00, +0x02,0x02,0x12,0x03,0x0b,0x02,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x0b,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01, +0x12,0x03,0x0b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x02,0x03,0x12,0x03,0x0b,0x11,0x12,0x0a, +0x0a,0x0a,0x02,0x04,0x01,0x12,0x04,0x0e,0x00,0x12, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,0x03, +0x0e,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02, +0x00,0x12,0x03,0x0f,0x02,0x10,0x0a,0x0c,0x0a,0x05, +0x04,0x01,0x02,0x00,0x05,0x12,0x03,0x0f,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x01,0x12, +0x03,0x0f,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x01, +0x02,0x00,0x03,0x12,0x03,0x0f,0x0e,0x0f,0x0a,0x0b, +0x0a,0x04,0x04,0x01,0x02,0x01,0x12,0x03,0x10,0x02, +0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x05, +0x12,0x03,0x10,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x01,0x02,0x01,0x01,0x12,0x03,0x10,0x09,0x0b,0x0a, +0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x03,0x12,0x03, +0x10,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02, +0x02,0x12,0x03,0x11,0x02,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x01,0x02,0x02,0x05,0x12,0x03,0x11,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x01,0x12, +0x03,0x11,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x01, +0x02,0x02,0x03,0x12,0x03,0x11,0x11,0x12,0x0a,0x0a, +0x0a,0x02,0x04,0x02,0x12,0x04,0x14,0x00,0x16,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01,0x12,0x03,0x14, +0x08,0x2b,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x00, +0x12,0x03,0x15,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, +0x02,0x02,0x00,0x05,0x12,0x03,0x15,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x01,0x12,0x03, +0x15,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, +0x00,0x03,0x12,0x03,0x15,0x16,0x17,0x0a,0x0a,0x0a, +0x02,0x04,0x03,0x12,0x04,0x18,0x00,0x1b,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x18,0x08, +0x2c,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12, +0x03,0x19,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03, +0x02,0x00,0x05,0x12,0x03,0x19,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x19, +0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00, +0x03,0x12,0x03,0x19,0x10,0x11,0x0a,0x0b,0x0a,0x04, +0x04,0x03,0x02,0x01,0x12,0x03,0x1a,0x02,0x13,0x0a, +0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03, +0x1a,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, +0x01,0x01,0x12,0x03,0x1a,0x09,0x0e,0x0a,0x0c,0x0a, +0x05,0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x1a,0x11, +0x12,0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1d, +0x00,0x20,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01, +0x12,0x03,0x1d,0x08,0x33,0x0a,0x0b,0x0a,0x04,0x04, +0x04,0x02,0x00,0x12,0x03,0x1e,0x02,0x12,0x0a,0x0c, +0x0a,0x05,0x04,0x04,0x02,0x00,0x05,0x12,0x03,0x1e, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00, +0x01,0x12,0x03,0x1e,0x09,0x0d,0x0a,0x0c,0x0a,0x05, +0x04,0x04,0x02,0x00,0x03,0x12,0x03,0x1e,0x10,0x11, +0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03, +0x1f,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, +0x01,0x05,0x12,0x03,0x1f,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x1f,0x09, +0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03, +0x12,0x03,0x1f,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04, +0x05,0x12,0x04,0x22,0x00,0x25,0x01,0x0a,0x0a,0x0a, +0x03,0x04,0x05,0x01,0x12,0x03,0x22,0x08,0x2f,0x0a, +0x0b,0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x23, +0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00, +0x05,0x12,0x03,0x23,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x05,0x02,0x00,0x01,0x12,0x03,0x23,0x09,0x0d, +0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12, +0x03,0x23,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x05, +0x02,0x01,0x12,0x03,0x24,0x02,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x24,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01, +0x12,0x03,0x24,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, +0x05,0x02,0x01,0x03,0x12,0x03,0x24,0x11,0x12,0x0a, +0x0a,0x0a,0x02,0x04,0x06,0x12,0x04,0x27,0x00,0x2c, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03, +0x27,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02, +0x00,0x12,0x03,0x28,0x02,0x27,0x0a,0x0c,0x0a,0x05, +0x04,0x06,0x02,0x00,0x06,0x12,0x03,0x28,0x02,0x17, +0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12, +0x03,0x28,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x00,0x03,0x12,0x03,0x28,0x25,0x26,0x0a,0x0b, +0x0a,0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x29,0x02, +0x28,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x06, +0x12,0x03,0x29,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, +0x06,0x02,0x01,0x01,0x12,0x03,0x29,0x18,0x23,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03, +0x29,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02, +0x02,0x12,0x03,0x2a,0x02,0x26,0x0a,0x0c,0x0a,0x05, +0x04,0x06,0x02,0x02,0x06,0x12,0x03,0x2a,0x02,0x17, +0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12, +0x03,0x2a,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x02,0x03,0x12,0x03,0x2a,0x24,0x25,0x0a,0x0b, +0x0a,0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2b,0x02, +0x27,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x06, +0x12,0x03,0x2b,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, +0x06,0x02,0x03,0x01,0x12,0x03,0x2b,0x18,0x22,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03, +0x2b,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12, +0x04,0x2e,0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x07,0x01,0x12,0x03,0x2e,0x08,0x2a,0x0a,0x0b,0x0a, +0x04,0x04,0x07,0x02,0x00,0x12,0x03,0x2f,0x02,0x18, +0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x05,0x12, +0x03,0x2f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07, +0x02,0x00,0x01,0x12,0x03,0x2f,0x09,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x07,0x02,0x00,0x03,0x12,0x03,0x2f, +0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x01, +0x12,0x03,0x30,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04, +0x07,0x02,0x01,0x05,0x12,0x03,0x30,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x01,0x12,0x03, +0x30,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, +0x01,0x03,0x12,0x03,0x30,0x17,0x18,0x0a,0x0b,0x0a, +0x04,0x04,0x07,0x02,0x02,0x12,0x03,0x31,0x02,0x17, +0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x05,0x12, +0x03,0x31,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07, +0x02,0x02,0x01,0x12,0x03,0x31,0x09,0x12,0x0a,0x0c, +0x0a,0x05,0x04,0x07,0x02,0x02,0x03,0x12,0x03,0x31, +0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x03, +0x12,0x03,0x32,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, +0x07,0x02,0x03,0x05,0x12,0x03,0x32,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x01,0x12,0x03, +0x32,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, +0x03,0x03,0x12,0x03,0x32,0x16,0x17,0x0a,0x0a,0x0a, +0x02,0x04,0x08,0x12,0x04,0x35,0x00,0x3a,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x08,0x01,0x12,0x03,0x35,0x08, +0x27,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x00,0x12, +0x03,0x36,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08, +0x02,0x00,0x05,0x12,0x03,0x36,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x08,0x02,0x00,0x01,0x12,0x03,0x36, +0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00, +0x03,0x12,0x03,0x36,0x16,0x17,0x0a,0x0b,0x0a,0x04, +0x04,0x08,0x02,0x01,0x12,0x03,0x37,0x02,0x19,0x0a, +0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x05,0x12,0x03, +0x37,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, +0x01,0x01,0x12,0x03,0x37,0x09,0x14,0x0a,0x0c,0x0a, +0x05,0x04,0x08,0x02,0x01,0x03,0x12,0x03,0x37,0x17, +0x18,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x02,0x12, +0x03,0x38,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x08, +0x02,0x02,0x05,0x12,0x03,0x38,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x08,0x02,0x02,0x01,0x12,0x03,0x38, +0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02, +0x03,0x12,0x03,0x38,0x15,0x16,0x0a,0x0b,0x0a,0x04, +0x04,0x08,0x02,0x03,0x12,0x03,0x39,0x02,0x18,0x0a, +0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x05,0x12,0x03, +0x39,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, +0x03,0x01,0x12,0x03,0x39,0x09,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x08,0x02,0x03,0x03,0x12,0x03,0x39,0x16, +0x17,0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,0x3c, +0x00,0x41,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01, +0x12,0x03,0x3c,0x08,0x2e,0x0a,0x0b,0x0a,0x04,0x04, +0x09,0x02,0x00,0x12,0x03,0x3d,0x02,0x18,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x00,0x05,0x12,0x03,0x3d, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00, +0x01,0x12,0x03,0x3d,0x09,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x09,0x02,0x00,0x03,0x12,0x03,0x3d,0x16,0x17, +0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x01,0x12,0x03, +0x3e,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, +0x01,0x05,0x12,0x03,0x3e,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x09,0x02,0x01,0x01,0x12,0x03,0x3e,0x09, +0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x03, +0x12,0x03,0x3e,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04, +0x09,0x02,0x02,0x12,0x03,0x3f,0x02,0x17,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x02,0x05,0x12,0x03,0x3f, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x02, +0x01,0x12,0x03,0x3f,0x09,0x12,0x0a,0x0c,0x0a,0x05, +0x04,0x09,0x02,0x02,0x03,0x12,0x03,0x3f,0x15,0x16, +0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x03,0x12,0x03, +0x40,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, +0x03,0x05,0x12,0x03,0x40,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x09,0x02,0x03,0x01,0x12,0x03,0x40,0x09, +0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x03, +0x12,0x03,0x40,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04, +0x0a,0x12,0x04,0x43,0x00,0x45,0x01,0x0a,0x0a,0x0a, +0x03,0x04,0x0a,0x01,0x12,0x03,0x43,0x08,0x25,0x0a, +0x0b,0x0a,0x04,0x04,0x0a,0x02,0x00,0x12,0x03,0x44, +0x02,0x2d,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00, +0x04,0x12,0x03,0x44,0x02,0x0a,0x0a,0x0c,0x0a,0x05, +0x04,0x0a,0x02,0x00,0x06,0x12,0x03,0x44,0x0b,0x20, +0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x01,0x12, +0x03,0x44,0x21,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x0a, +0x02,0x00,0x03,0x12,0x03,0x44,0x2b,0x2c,0x0a,0x0a, +0x0a,0x02,0x04,0x0b,0x12,0x04,0x47,0x00,0x4a,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x0b,0x01,0x12,0x03,0x47, +0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02,0x00, +0x12,0x03,0x48,0x02,0x16,0x0a,0x0c,0x0a,0x05,0x04, +0x0b,0x02,0x00,0x05,0x12,0x03,0x48,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x01,0x12,0x03, +0x48,0x09,0x11,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02, +0x00,0x03,0x12,0x03,0x48,0x14,0x15,0x0a,0x0b,0x0a, +0x04,0x04,0x0b,0x02,0x01,0x12,0x03,0x49,0x02,0x1f, +0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x06,0x12, +0x03,0x49,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0b, +0x02,0x01,0x01,0x12,0x03,0x49,0x15,0x1a,0x0a,0x0c, +0x0a,0x05,0x04,0x0b,0x02,0x01,0x03,0x12,0x03,0x49, +0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12,0x04, +0x4c,0x00,0x4f,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0c, +0x01,0x12,0x03,0x4c,0x08,0x21,0x0a,0x0b,0x0a,0x04, +0x04,0x0c,0x02,0x00,0x12,0x03,0x4d,0x02,0x13,0x0a, +0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x05,0x12,0x03, +0x4d,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02, +0x00,0x01,0x12,0x03,0x4d,0x09,0x0e,0x0a,0x0c,0x0a, +0x05,0x04,0x0c,0x02,0x00,0x03,0x12,0x03,0x4d,0x11, +0x12,0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01,0x12, +0x03,0x4e,0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0c, +0x02,0x01,0x06,0x12,0x03,0x4e,0x02,0x14,0x0a,0x0c, +0x0a,0x05,0x04,0x0c,0x02,0x01,0x01,0x12,0x03,0x4e, +0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01, +0x03,0x12,0x03,0x4e,0x1d,0x1e,0x0a,0x0a,0x0a,0x02, +0x04,0x0d,0x12,0x04,0x51,0x00,0x54,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x0d,0x01,0x12,0x03,0x51,0x08,0x29, +0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02,0x00,0x12,0x03, +0x52,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02, +0x00,0x05,0x12,0x03,0x52,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x0d,0x02,0x00,0x01,0x12,0x03,0x52,0x09, +0x15,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x03, +0x12,0x03,0x52,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04, +0x0d,0x02,0x01,0x12,0x03,0x53,0x02,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x0d,0x02,0x01,0x05,0x12,0x03,0x53, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01, +0x01,0x12,0x03,0x53,0x09,0x0e,0x0a,0x0c,0x0a,0x05, +0x04,0x0d,0x02,0x01,0x03,0x12,0x03,0x53,0x11,0x12, +0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, }; static const char file_name[] = "kinematics.proto"; static const char wpi_proto_ProtobufChassisSpeeds_name[] = "wpi.proto.ProtobufChassisSpeeds"; diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h index cd31c0c358c..97fff24e554 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h @@ -147,7 +147,7 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { static pb_filedesc_t file_descriptor(void) noexcept; double acceleration; - double angular_acceleration; + double angle; } wpi_proto_ProtobufSwerveModuleAccelerations; @@ -217,7 +217,7 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { #define wpi_proto_ProtobufSwerveModuleState_speed_tag 1 #define wpi_proto_ProtobufSwerveModuleState_angle_tag 2 #define wpi_proto_ProtobufSwerveModuleAccelerations_acceleration_tag 1 -#define wpi_proto_ProtobufSwerveModuleAccelerations_angular_acceleration_tag 2 +#define wpi_proto_ProtobufSwerveModuleAccelerations_angle_tag 2 /* Struct field encoding specification for nanopb */ #define wpi_proto_ProtobufChassisSpeeds_FIELDLIST(X, a) \ @@ -315,7 +315,7 @@ X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2) #define wpi_proto_ProtobufSwerveModuleAccelerations_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, DOUBLE, acceleration, 1) \ -X(a, STATIC, SINGULAR, DOUBLE, angular_acceleration, 2) +X(a, STATIC, SINGULAR, DOUBLE, angle, 2) #define wpi_proto_ProtobufSwerveModuleAccelerations_CALLBACK NULL #define wpi_proto_ProtobufSwerveModuleAccelerations_DEFAULT NULL diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java index 707519cc6b8..27cf11b7624 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java @@ -5,12 +5,11 @@ package edu.wpi.first.math.kinematics; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.SwerveModuleAccelerationsProto; import edu.wpi.first.math.kinematics.struct.SwerveModuleAccelerationsStruct; -import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -25,8 +24,8 @@ public class SwerveModuleAccelerations /** Acceleration of the wheel of the module in meters per second squared. */ public double acceleration; - /** Angular acceleration of the module in radians per second squared. */ - public double angularAcceleration; + /** Angle of the acceleration vector. */ + public Rotation2d angle = new Rotation2d(); /** SwerveModuleAccelerations protobuf for serialization. */ public static final SwerveModuleAccelerationsProto proto = new SwerveModuleAccelerationsProto(); @@ -36,7 +35,7 @@ public class SwerveModuleAccelerations new SwerveModuleAccelerationsStruct(); /** - * Constructs a SwerveModuleAccelerations with zeros for acceleration and angular acceleration. + * Constructs a SwerveModuleAccelerations with zeros for acceleration and angle. */ public SwerveModuleAccelerations() {} @@ -44,80 +43,21 @@ public SwerveModuleAccelerations() {} * Constructs a SwerveModuleAccelerations. * * @param acceleration The acceleration of the wheel of the module in meters per second squared. - * @param angularAcceleration The angular acceleration of the module in radians per second - * squared. + * @param angle The angle of the acceleration vector. */ - public SwerveModuleAccelerations(double acceleration, double angularAcceleration) { + public SwerveModuleAccelerations(double acceleration, Rotation2d angle) { this.acceleration = acceleration; - this.angularAcceleration = angularAcceleration; + this.angle = angle; } /** * Constructs a SwerveModuleAccelerations. * * @param acceleration The acceleration of the wheel of the module. - * @param angularAcceleration The angular acceleration of the module. + * @param angle The angle of the acceleration vector. */ - public SwerveModuleAccelerations( - LinearAcceleration acceleration, AngularAcceleration angularAcceleration) { - this( - acceleration.in(MetersPerSecondPerSecond), - angularAcceleration.in(RadiansPerSecondPerSecond)); - } - - /** - * Adds two SwerveModuleAccelerations and returns the sum. - * - * @param other The SwerveModuleAccelerations to add. - * @return The sum of the SwerveModuleAccelerations. - */ - public SwerveModuleAccelerations plus(SwerveModuleAccelerations other) { - return new SwerveModuleAccelerations( - acceleration + other.acceleration, angularAcceleration + other.angularAcceleration); - } - - /** - * Subtracts the other SwerveModuleAccelerations from the current SwerveModuleAccelerations and - * returns the difference. - * - * @param other The SwerveModuleAccelerations to subtract. - * @return The difference between the two SwerveModuleAccelerations. - */ - public SwerveModuleAccelerations minus(SwerveModuleAccelerations other) { - return new SwerveModuleAccelerations( - acceleration - other.acceleration, angularAcceleration - other.angularAcceleration); - } - - /** - * Returns the inverse of the current SwerveModuleAccelerations. This is equivalent to negating - * all components of the SwerveModuleAccelerations. - * - * @return The inverse of the current SwerveModuleAccelerations. - */ - public SwerveModuleAccelerations unaryMinus() { - return new SwerveModuleAccelerations(-acceleration, -angularAcceleration); - } - - /** - * Multiplies the SwerveModuleAccelerations by a scalar and returns the new - * SwerveModuleAccelerations. - * - * @param scalar The scalar to multiply by. - * @return The scaled SwerveModuleAccelerations. - */ - public SwerveModuleAccelerations times(double scalar) { - return new SwerveModuleAccelerations(acceleration * scalar, angularAcceleration * scalar); - } - - /** - * Divides the SwerveModuleAccelerations by a scalar and returns the new - * SwerveModuleAccelerations. - * - * @param scalar The scalar to divide by. - * @return The scaled SwerveModuleAccelerations. - */ - public SwerveModuleAccelerations div(double scalar) { - return new SwerveModuleAccelerations(acceleration / scalar, angularAcceleration / scalar); + public SwerveModuleAccelerations(LinearAcceleration acceleration, Rotation2d angle) { + this(acceleration.in(MetersPerSecondPerSecond), angle); } /** @@ -134,24 +74,24 @@ public SwerveModuleAccelerations interpolate(SwerveModuleAccelerations endValue, return new SwerveModuleAccelerations( acceleration + t * (endValue.acceleration - acceleration), - angularAcceleration + t * (endValue.angularAcceleration - angularAcceleration)); + angle.interpolate(endValue.angle, t)); } @Override public boolean equals(Object obj) { return obj instanceof SwerveModuleAccelerations other && Math.abs(other.acceleration - acceleration) < 1E-9 - && Math.abs(other.angularAcceleration - angularAcceleration) < 1E-9; + && angle.equals(other.angle); } @Override public int hashCode() { - return Objects.hash(acceleration, angularAcceleration); + return Objects.hash(acceleration, angle); } /** * Compares two swerve module accelerations. One swerve module is "greater" than the other if its - * acceleration is higher than the other. + * acceleration magnitude is higher than the other. * * @param other The other swerve module. * @return 1 if this is greater, 0 if both are equal, -1 if other is greater. @@ -164,7 +104,7 @@ public int compareTo(SwerveModuleAccelerations other) { @Override public String toString() { return String.format( - "SwerveModuleAccelerations(Acceleration: %.2f m/s², Angular Acceleration: %.2f rad/s²)", - acceleration, angularAcceleration); + "SwerveModuleAccelerations(Acceleration: %.2f m/s², Angle: %s)", + acceleration, angle); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java index 13adba8a962..c5228892166 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.kinematics.proto; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleAccelerations; import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModuleAccelerations; import edu.wpi.first.util.protobuf.Protobuf; @@ -28,12 +29,12 @@ public ProtobufSwerveModuleAccelerations createMessage() { @Override public SwerveModuleAccelerations unpack(ProtobufSwerveModuleAccelerations msg) { - return new SwerveModuleAccelerations(msg.getAcceleration(), msg.getAngularAcceleration()); + return new SwerveModuleAccelerations(msg.getAcceleration(), new Rotation2d(msg.getAngle())); } @Override public void pack(ProtobufSwerveModuleAccelerations msg, SwerveModuleAccelerations value) { msg.setAcceleration(value.acceleration); - msg.setAngularAcceleration(value.angularAcceleration); + msg.setAngle(value.angle.getRadians()); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java index cd8752459ab..aa4a37c7fbb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.kinematics.struct; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleAccelerations; import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; @@ -26,19 +27,19 @@ public int getSize() { @Override public String getSchema() { - return "double acceleration;double angular_acceleration"; + return "double acceleration;double angle"; } @Override public SwerveModuleAccelerations unpack(ByteBuffer bb) { double acceleration = bb.getDouble(); - double angularAcceleration = bb.getDouble(); - return new SwerveModuleAccelerations(acceleration, angularAcceleration); + double angle = bb.getDouble(); + return new SwerveModuleAccelerations(acceleration, new Rotation2d(angle)); } @Override public void pack(ByteBuffer bb, SwerveModuleAccelerations value) { bb.putDouble(value.acceleration); - bb.putDouble(value.angularAcceleration); + bb.putDouble(value.angle.getRadians()); } } diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp index 9a9808fae8d..a433096a33e 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp @@ -15,7 +15,7 @@ std::optional wpi::Protobuf::Pack(OutputStream& stream, const frc::SwerveModuleAccelerations& value) { wpi_proto_ProtobufSwerveModuleAccelerations msg{ .acceleration = value.acceleration.value(), - .angular_acceleration = value.angularAcceleration.value(), + .angle = value.angle.Radians().value(), }; return stream.Encode(msg); } diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp index 94437430122..797237de17d 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp @@ -11,19 +11,19 @@ frc::SwerveModuleAccelerations wpi::Struct::Unpack( std::span data) { constexpr size_t kAccelerationOff = 0; - constexpr size_t kAngularAccelerationOff = kAccelerationOff + 8; + constexpr size_t kAngleOff = kAccelerationOff + 8; return frc::SwerveModuleAccelerations{ units::meters_per_second_squared_t{ wpi::UnpackStruct(data)}, - units::radians_per_second_squared_t{ - wpi::UnpackStruct(data)}, + frc::Rotation2d{units::radian_t{ + wpi::UnpackStruct(data)}}, }; } void wpi::Struct::Pack( std::span data, const frc::SwerveModuleAccelerations& value) { constexpr size_t kAccelerationOff = 0; - constexpr size_t kAngularAccelerationOff = kAccelerationOff + 8; + constexpr size_t kAngleOff = kAccelerationOff + 8; wpi::PackStruct(data, value.acceleration.value()); - wpi::PackStruct(data, value.angularAcceleration.value()); + wpi::PackStruct(data, value.angle.Radians().value()); } diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h index b02e353f4fd..ee8032af21c 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h @@ -6,8 +6,8 @@ #include +#include "frc/geometry/Rotation2d.h" #include "units/acceleration.h" -#include "units/angular_acceleration.h" #include "units/math.h" namespace frc { @@ -21,9 +21,9 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { units::meters_per_second_squared_t acceleration = 0_mps_sq; /** - * Angular acceleration of the module. + * Angle of the acceleration vector. */ - units::radians_per_second_squared_t angularAcceleration = 0_rad_per_s_sq; + Rotation2d angle; /** * Checks equality between this SwerveModuleAccelerations and another object. @@ -33,18 +33,30 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { */ constexpr bool operator==(const SwerveModuleAccelerations& other) const { return units::math::abs(acceleration - other.acceleration) < 1E-9_mps_sq && - units::math::abs(angularAcceleration - other.angularAcceleration) < 1E-9_rad_per_s_sq; + angle == other.angle; } /** * Adds two SwerveModuleAccelerations and returns the sum. + * Note: This performs vector addition of the accelerations. * * @param other The SwerveModuleAccelerations to add. * @return The sum of the SwerveModuleAccelerations. */ - constexpr SwerveModuleAccelerations operator+( - const SwerveModuleAccelerations& other) const { - return {acceleration + other.acceleration, angularAcceleration + other.angularAcceleration}; + SwerveModuleAccelerations operator+(const SwerveModuleAccelerations& other) const { + // Convert to Cartesian coordinates, add, then convert back + auto thisX = acceleration * angle.Cos(); + auto thisY = acceleration * angle.Sin(); + auto otherX = other.acceleration * other.angle.Cos(); + auto otherY = other.acceleration * other.angle.Sin(); + + auto sumX = thisX + otherX; + auto sumY = thisY + otherY; + + auto resultAcceleration = units::math::hypot(sumX, sumY); + auto resultAngle = Rotation2d{sumX.value(), sumY.value()}; + + return {resultAcceleration, resultAngle}; } /** @@ -54,20 +66,18 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * @param other The SwerveModuleAccelerations to subtract. * @return The difference between the two SwerveModuleAccelerations. */ - constexpr SwerveModuleAccelerations operator-( - const SwerveModuleAccelerations& other) const { - return *this + -other; + SwerveModuleAccelerations operator-(const SwerveModuleAccelerations& other) const { + return *this + (-other); } /** * Returns the inverse of the current SwerveModuleAccelerations. - * This is equivalent to negating all components of the - * SwerveModuleAccelerations. + * This is equivalent to negating the acceleration magnitude. * * @return The inverse of the current SwerveModuleAccelerations. */ constexpr SwerveModuleAccelerations operator-() const { - return {-acceleration, -angularAcceleration}; + return {-acceleration, angle + Rotation2d{180_deg}}; } /** @@ -78,7 +88,10 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * @return The scaled SwerveModuleAccelerations. */ constexpr SwerveModuleAccelerations operator*(double scalar) const { - return {scalar * acceleration, scalar * angularAcceleration}; + if (scalar < 0) { + return {-scalar * acceleration, angle + Rotation2d{180_deg}}; + } + return {scalar * acceleration, angle}; } /** diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h index 70101d64638..59c389b5478 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h @@ -14,7 +14,7 @@ struct WPILIB_DLLEXPORT wpi::Struct { static constexpr std::string_view GetTypeName() { return "SwerveModuleAccelerations"; } static constexpr size_t GetSize() { return 16; } static constexpr std::string_view GetSchema() { - return "double acceleration;double angular_acceleration"; + return "double acceleration;double angle"; } static frc::SwerveModuleAccelerations Unpack(std::span data); diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto index 68fe600dd19..82b95ff585e 100644 --- a/wpimath/src/main/proto/kinematics.proto +++ b/wpimath/src/main/proto/kinematics.proto @@ -81,5 +81,5 @@ message ProtobufSwerveModuleState { message ProtobufSwerveModuleAccelerations { double acceleration = 1; - double angular_acceleration = 2; + double angle = 2; } From b5ee68db041c35162b8278d9919fbe7f140b6348 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 22:10:09 -0400 Subject: [PATCH 18/82] refactor: update SwerveModuleAccelerations to use Rotation2d instead of angularAcceleration Signed-off-by: Zach Harel --- .../edu/wpi/first/math/kinematics/SwerveDriveKinematics.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index bab35cb7bd0..ad4e3e0c26b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -469,7 +469,7 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( && chassisAccelerations.ay == 0.0 && chassisAccelerations.alpha == 0.0) { for (int i = 0; i < m_numModules; i++) { - moduleAccelerations[i] = new SwerveModuleAccelerations(0.0, 0.0); + moduleAccelerations[i] = new SwerveModuleAccelerations(0.0, Rotation2d.kZero); } return moduleAccelerations; } @@ -504,7 +504,7 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( double angularAcceleration = 0.0; moduleAccelerations[i] = - new SwerveModuleAccelerations(linearAcceleration, angularAcceleration); + new SwerveModuleAccelerations(linearAcceleration, new Rotation2d(x, y)); } return moduleAccelerations; From 3156b9f45f701113a9e2bf2d76b9647cc2db7ebe Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 Aug 2025 22:24:11 -0400 Subject: [PATCH 19/82] refactor: update SwerveDriveKinematics to use angle instead of angularAcceleration and then Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematics.java | 15 +-- .../kinematics/SwerveModuleAccelerations.java | 7 +- .../kinematics/SwerveDriveKinematicsTest.java | 52 ++++++----- .../SwerveModuleAccelerationsTest.java | 91 ++++--------------- 4 files changed, 56 insertions(+), 109 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index ad4e3e0c26b..de2bf0f2467 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -498,13 +498,8 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( // The linear acceleration is the magnitude of the acceleration vector double linearAcceleration = Math.hypot(x, y); - // Angular acceleration represents how fast the module angle is changing - // This would typically come from higher-level control algorithms - // For basic kinematics, we set it to 0 (constant steering angle) - double angularAcceleration = 0.0; - moduleAccelerations[i] = - new SwerveModuleAccelerations(linearAcceleration, new Rotation2d(x, y)); + new SwerveModuleAccelerations(linearAcceleration, Rotation2d.fromRadians(Math.atan2(y, x))); } return moduleAccelerations; @@ -551,13 +546,9 @@ public ChassisAccelerations toChassisAccelerations( for (int i = 0; i < m_numModules; i++) { var module = moduleAccelerations[i]; - // For forward kinematics, we need to know the current steering angle of each module - // Since we don't have it directly in the accelerations, we'll use the stored headings - // In practice, this would come from the current module state - Rotation2d moduleAngle = m_moduleHeadings[i]; - moduleAccelerationsMatrix.set(i * 2, 0, module.acceleration * moduleAngle.getCos()); - moduleAccelerationsMatrix.set(i * 2 + 1, module.acceleration * moduleAngle.getSin()); + moduleAccelerationsMatrix.set(i * 2, 0, module.acceleration * module.angle.getCos()); + moduleAccelerationsMatrix.set(i * 2 + 1, module.acceleration * module.angle.getSin()); } var chassisAccelerationsVector = m_forwardKinematics.mult(moduleAccelerationsMatrix); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java index 27cf11b7624..466bcde061e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerations.java @@ -34,9 +34,7 @@ public class SwerveModuleAccelerations public static final SwerveModuleAccelerationsStruct struct = new SwerveModuleAccelerationsStruct(); - /** - * Constructs a SwerveModuleAccelerations with zeros for acceleration and angle. - */ + /** Constructs a SwerveModuleAccelerations with zeros for acceleration and angle. */ public SwerveModuleAccelerations() {} /** @@ -104,7 +102,6 @@ public int compareTo(SwerveModuleAccelerations other) { @Override public String toString() { return String.format( - "SwerveModuleAccelerations(Acceleration: %.2f m/s², Angle: %s)", - acceleration, angle); + "SwerveModuleAccelerations(Acceleration: %.2f m/s², Angle: %s)", acceleration, angle); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index e4fb5f2c5bd..12a626de8b4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -418,6 +418,10 @@ void testTurnInPlaceInverseAccelerations() { trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second squared, our wheels must trace out 1 rotation (or 106.63 inches) per second squared. + + For turn-in-place, each module accelerates tangentially to the circle, so the angle + of acceleration should match the expected turning angles: + FL: 135°, FR: 45°, BL: -135°, BR: -45° */ assertAll( @@ -425,32 +429,29 @@ void testTurnInPlaceInverseAccelerations() { () -> assertEquals(106.63, moduleAccelerations[1].acceleration, 0.1), () -> assertEquals(106.63, moduleAccelerations[2].acceleration, 0.1), () -> assertEquals(106.63, moduleAccelerations[3].acceleration, 0.1), - () -> assertEquals(0.0, moduleAccelerations[0].angularAcceleration, kEpsilon), - () -> assertEquals(0.0, moduleAccelerations[1].angularAcceleration, kEpsilon), - () -> assertEquals(0.0, moduleAccelerations[2].angularAcceleration, kEpsilon), - () -> assertEquals(0.0, moduleAccelerations[3].angularAcceleration, kEpsilon)); + () -> assertEquals(135.0, moduleAccelerations[0].angle.getDegrees(), 0.1), + () -> assertEquals(45.0, moduleAccelerations[1].angle.getDegrees(), 0.1), + () -> assertEquals(-135.0, moduleAccelerations[2].angle.getDegrees(), 0.1), + () -> assertEquals(-45.0, moduleAccelerations[3].angle.getDegrees(), 0.1)); } @Test void testTurnInPlaceForwardAccelerations() { - SwerveModuleAccelerations flAccel = new SwerveModuleAccelerations(106.629, 0.0); - SwerveModuleAccelerations frAccel = new SwerveModuleAccelerations(106.629, 0.0); - SwerveModuleAccelerations blAccel = new SwerveModuleAccelerations(106.629, 0.0); - SwerveModuleAccelerations brAccel = new SwerveModuleAccelerations(106.629, 0.0); - - // Set the headings to match the turn-in-place configuration - m_kinematics.resetHeadings( - Rotation2d.fromDegrees(135), - Rotation2d.fromDegrees(45), - Rotation2d.fromDegrees(-135), - Rotation2d.fromDegrees(-45)); + SwerveModuleAccelerations flAccel = + new SwerveModuleAccelerations(106.629, Rotation2d.fromDegrees(135)); + SwerveModuleAccelerations frAccel = + new SwerveModuleAccelerations(106.629, Rotation2d.fromDegrees(45)); + SwerveModuleAccelerations blAccel = + new SwerveModuleAccelerations(106.629, Rotation2d.fromDegrees(-135)); + SwerveModuleAccelerations brAccel = + new SwerveModuleAccelerations(106.629, Rotation2d.fromDegrees(-45)); var chassisAccelerations = m_kinematics.toChassisAccelerations(flAccel, frAccel, blAccel, brAccel); assertAll( - () -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon), - () -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon), + () -> assertEquals(0.0, chassisAccelerations.ax, 0.1), + () -> assertEquals(0.0, chassisAccelerations.ay, 0.1), () -> assertEquals(2 * Math.PI, chassisAccelerations.alpha, 0.1)); } @@ -459,14 +460,23 @@ void testOffCenterRotationInverseAccelerations() { ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 1); var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations, m_fl); + /* + When rotating about the front-left module position, each module accelerates + tangentially to its circular path. The angles should be: + FL: stationary (0 acceleration) + FR: straight forward (0°) + BL: straight right (-90°) + BR: -45° (tangent to the diagonal path) + */ + assertAll( () -> assertEquals(0, moduleAccelerations[0].acceleration, 0.1), () -> assertEquals(24.0, moduleAccelerations[1].acceleration, 0.1), () -> assertEquals(24.0, moduleAccelerations[2].acceleration, 0.1), () -> assertEquals(33.941, moduleAccelerations[3].acceleration, 0.1), - () -> assertEquals(0.0, moduleAccelerations[0].angularAcceleration, kEpsilon), - () -> assertEquals(0.0, moduleAccelerations[1].angularAcceleration, kEpsilon), - () -> assertEquals(0.0, moduleAccelerations[2].angularAcceleration, kEpsilon), - () -> assertEquals(0.0, moduleAccelerations[3].angularAcceleration, kEpsilon)); + () -> assertEquals(0.0, moduleAccelerations[0].angle.getDegrees(), 0.1), // FL stationary + () -> assertEquals(0.0, moduleAccelerations[1].angle.getDegrees(), 0.1), // FR forward + () -> assertEquals(-90.0, moduleAccelerations[2].angle.getDegrees(), 0.1), // BL right + () -> assertEquals(-45.0, moduleAccelerations[3].angle.getDegrees(), 0.1)); // BR diagonal } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java index 3abb67b87d7..3db682cc925 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java @@ -5,12 +5,12 @@ package edu.wpi.first.math.kinematics; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.geometry.Rotation2d; import org.junit.jupiter.api.Test; class SwerveModuleAccelerationsTest { @@ -22,34 +22,34 @@ void testDefaultConstructor() { assertAll( () -> assertEquals(0.0, moduleAccelerations.acceleration, kEpsilon), - () -> assertEquals(0.0, moduleAccelerations.angularAcceleration, kEpsilon)); + () -> assertEquals(0.0, moduleAccelerations.angle.getRadians(), kEpsilon)); } @Test void testParameterizedConstructor() { - var moduleAccelerations = new SwerveModuleAccelerations(2.5, 1.5); + var moduleAccelerations = new SwerveModuleAccelerations(2.5, new Rotation2d(1.5)); assertAll( () -> assertEquals(2.5, moduleAccelerations.acceleration, kEpsilon), - () -> assertEquals(1.5, moduleAccelerations.angularAcceleration, kEpsilon)); + () -> assertEquals(1.5, moduleAccelerations.angle.getRadians(), kEpsilon)); } @Test void testMeasureConstructor() { var acceleration = MetersPerSecondPerSecond.of(3.0); - var angularAcceleration = RadiansPerSecondPerSecond.of(2.0); - var moduleAccelerations = new SwerveModuleAccelerations(acceleration, angularAcceleration); + var angle = new Rotation2d(2.0); + var moduleAccelerations = new SwerveModuleAccelerations(acceleration, angle); assertAll( () -> assertEquals(3.0, moduleAccelerations.acceleration, kEpsilon), - () -> assertEquals(2.0, moduleAccelerations.angularAcceleration, kEpsilon)); + () -> assertEquals(2.0, moduleAccelerations.angle.getRadians(), kEpsilon)); } @Test void testEquals() { - var moduleAccelerations1 = new SwerveModuleAccelerations(2.0, 1.5); - var moduleAccelerations2 = new SwerveModuleAccelerations(2.0, 1.5); - var moduleAccelerations3 = new SwerveModuleAccelerations(2.1, 1.5); + var moduleAccelerations1 = new SwerveModuleAccelerations(2.0, new Rotation2d(1.5)); + var moduleAccelerations2 = new SwerveModuleAccelerations(2.0, new Rotation2d(1.5)); + var moduleAccelerations3 = new SwerveModuleAccelerations(2.1, new Rotation2d(1.5)); assertEquals(moduleAccelerations1, moduleAccelerations2); assertNotEquals(moduleAccelerations1, moduleAccelerations3); @@ -57,104 +57,53 @@ void testEquals() { @Test void testCompareTo() { - var slower = new SwerveModuleAccelerations(1.0, 2.0); - var faster = new SwerveModuleAccelerations(2.0, 1.0); + var slower = new SwerveModuleAccelerations(1.0, new Rotation2d(2.0)); + var faster = new SwerveModuleAccelerations(2.0, new Rotation2d(1.0)); assertTrue(slower.compareTo(faster) < 0); assertTrue(faster.compareTo(slower) > 0); assertEquals(0, slower.compareTo(slower)); } - @Test - void testPlus() { - final var left = new SwerveModuleAccelerations(1.0, 0.5); - final var right = new SwerveModuleAccelerations(2.0, 1.5); - - final var moduleAccelerations = left.plus(right); - - assertAll( - () -> assertEquals(3.0, moduleAccelerations.acceleration), - () -> assertEquals(2.0, moduleAccelerations.angularAcceleration)); - } - - @Test - void testMinus() { - final var left = new SwerveModuleAccelerations(3.0, 2.5); - final var right = new SwerveModuleAccelerations(1.0, 0.5); - - final var moduleAccelerations = left.minus(right); - - assertAll( - () -> assertEquals(2.0, moduleAccelerations.acceleration), - () -> assertEquals(2.0, moduleAccelerations.angularAcceleration)); - } - - @Test - void testUnaryMinus() { - final var moduleAccelerations = new SwerveModuleAccelerations(1.5, -2.0).unaryMinus(); - - assertAll( - () -> assertEquals(-1.5, moduleAccelerations.acceleration), - () -> assertEquals(2.0, moduleAccelerations.angularAcceleration)); - } - - @Test - void testMultiplication() { - final var moduleAccelerations = new SwerveModuleAccelerations(2.0, 1.5).times(2.0); - - assertAll( - () -> assertEquals(4.0, moduleAccelerations.acceleration), - () -> assertEquals(3.0, moduleAccelerations.angularAcceleration)); - } - - @Test - void testDivision() { - final var moduleAccelerations = new SwerveModuleAccelerations(4.0, 2.0).div(2.0); - - assertAll( - () -> assertEquals(2.0, moduleAccelerations.acceleration), - () -> assertEquals(1.0, moduleAccelerations.angularAcceleration)); - } - @Test void testInterpolate() { - final var start = new SwerveModuleAccelerations(1.0, 2.0); - final var end = new SwerveModuleAccelerations(5.0, 6.0); + final var start = new SwerveModuleAccelerations(1.0, new Rotation2d(0.0)); + final var end = new SwerveModuleAccelerations(5.0, new Rotation2d(Math.PI / 2)); // Test interpolation at t=0 (should return start) final var atStart = start.interpolate(end, 0.0); assertAll( () -> assertEquals(1.0, atStart.acceleration, kEpsilon), - () -> assertEquals(2.0, atStart.angularAcceleration, kEpsilon)); + () -> assertEquals(0.0, atStart.angle.getRadians(), kEpsilon)); // Test interpolation at t=1 (should return end) final var atEnd = start.interpolate(end, 1.0); assertAll( () -> assertEquals(5.0, atEnd.acceleration, kEpsilon), - () -> assertEquals(6.0, atEnd.angularAcceleration, kEpsilon)); + () -> assertEquals(Math.PI / 2, atEnd.angle.getRadians(), kEpsilon)); // Test interpolation at t=0.5 (should return midpoint) final var atMidpoint = start.interpolate(end, 0.5); assertAll( () -> assertEquals(3.0, atMidpoint.acceleration, kEpsilon), - () -> assertEquals(4.0, atMidpoint.angularAcceleration, kEpsilon)); + () -> assertEquals(Math.PI / 4, atMidpoint.angle.getRadians(), kEpsilon)); // Test interpolation at t=0.25 final var atQuarter = start.interpolate(end, 0.25); assertAll( () -> assertEquals(2.0, atQuarter.acceleration, kEpsilon), - () -> assertEquals(3.0, atQuarter.angularAcceleration, kEpsilon)); + () -> assertEquals(Math.PI / 8, atQuarter.angle.getRadians(), kEpsilon)); // Test clamping: t < 0 should clamp to 0 final var belowRange = start.interpolate(end, -0.5); assertAll( () -> assertEquals(1.0, belowRange.acceleration, kEpsilon), - () -> assertEquals(2.0, belowRange.angularAcceleration, kEpsilon)); + () -> assertEquals(0.0, belowRange.angle.getRadians(), kEpsilon)); // Test clamping: t > 1 should clamp to 1 final var aboveRange = start.interpolate(end, 1.5); assertAll( () -> assertEquals(5.0, aboveRange.acceleration, kEpsilon), - () -> assertEquals(6.0, aboveRange.angularAcceleration, kEpsilon)); + () -> assertEquals(Math.PI / 2, aboveRange.angle.getRadians(), kEpsilon)); } } From 2531ceb2e559e9d7d53755358208903ce05c42ac Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 22 Aug 2025 10:47:49 -0400 Subject: [PATCH 20/82] refactor: include the proper second order kinematics calculation for swerve (which includes angular velocity due to centripetal acceleration component) Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematics.java | 86 +++++++++++++------ 1 file changed, 58 insertions(+), 28 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index de2bf0f2467..0e5bae3371c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -46,8 +46,10 @@ public class SwerveDriveKinematics implements Kinematics, ProtobufSerializable, StructSerializable { - private final SimpleMatrix m_inverseKinematics; - private final SimpleMatrix m_forwardKinematics; + private final SimpleMatrix m_firstOrderInverseKinematics; + private final SimpleMatrix m_firstOrderForwardKinematics; + private final SimpleMatrix m_secondOrderInverseKinematics; + private final SimpleMatrix m_secondOrderForwardKinematics; private final int m_numModules; private final Translation2d[] m_modules; @@ -72,13 +74,22 @@ public SwerveDriveKinematics(Translation2d... moduleTranslations) { m_modules = Arrays.copyOf(moduleTranslations, m_numModules); m_moduleHeadings = new Rotation2d[m_numModules]; Arrays.fill(m_moduleHeadings, Rotation2d.kZero); - m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3); + m_firstOrderInverseKinematics = new SimpleMatrix(m_numModules * 2, 3); + m_secondOrderInverseKinematics = new SimpleMatrix(m_numModules * 2, 4); for (int i = 0; i < m_numModules; i++) { - m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY()); - m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX()); + m_firstOrderInverseKinematics.setRow( + i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY()); + m_firstOrderInverseKinematics.setRow( + i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX()); + + m_secondOrderInverseKinematics.setRow( + i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY()); + m_secondOrderInverseKinematics.setRow( + i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX()); } - m_forwardKinematics = m_inverseKinematics.pseudoInverse(); + m_firstOrderForwardKinematics = m_firstOrderInverseKinematics.pseudoInverse(); + m_secondOrderForwardKinematics = m_secondOrderInverseKinematics.pseudoInverse(); MathSharedStore.reportUsage("SwerveDriveKinematics", ""); } @@ -133,9 +144,9 @@ public SwerveModuleState[] toSwerveModuleStates( if (!centerOfRotation.equals(m_prevCoR)) { for (int i = 0; i < m_numModules; i++) { - m_inverseKinematics.setRow( + m_firstOrderInverseKinematics.setRow( i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotation.getY()); - m_inverseKinematics.setRow( + m_firstOrderInverseKinematics.setRow( i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX() - centerOfRotation.getX()); } m_prevCoR = centerOfRotation; @@ -144,7 +155,7 @@ public SwerveModuleState[] toSwerveModuleStates( var chassisSpeedsVector = new SimpleMatrix(3, 1); chassisSpeedsVector.setColumn(0, 0, chassisSpeeds.vx, chassisSpeeds.vy, chassisSpeeds.omega); - var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector); + var moduleStatesMatrix = m_firstOrderInverseKinematics.mult(chassisSpeedsVector); for (int i = 0; i < m_numModules; i++) { double x = moduleStatesMatrix.get(i * 2, 0); @@ -201,7 +212,7 @@ public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) { moduleStatesMatrix.set(i * 2 + 1, module.speed * module.angle.getSin()); } - var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix); + var chassisSpeedsVector = m_firstOrderForwardKinematics.mult(moduleStatesMatrix); return new ChassisSpeeds( chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0), @@ -232,7 +243,7 @@ public Twist2d toTwist2d(SwerveModulePosition... moduleDeltas) { moduleDeltaMatrix.set(i * 2 + 1, module.distance * module.angle.getSin()); } - var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix); + var chassisDeltaVector = m_firstOrderForwardKinematics.mult(moduleDeltaMatrix); return new Twist2d( chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0)); } @@ -456,13 +467,16 @@ public static final SwerveDriveKinematicsStruct getStruct(int numModules) { * stationary), the previously calculated module angle will be maintained. * * @param chassisAccelerations The desired chassis accelerations. + * @param angularVelocity The desired robot angular velocity. * @param centerOfRotation The center of rotation. For example, if you set the center of rotation * at one corner of the robot and provide a chassis acceleration that only has a dtheta * component, the robot will rotate around that corner. * @return An array containing the module accelerations. */ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( - ChassisAccelerations chassisAccelerations, Translation2d centerOfRotation) { + ChassisAccelerations chassisAccelerations, + double angularVelocity, + Translation2d centerOfRotation) { var moduleAccelerations = new SwerveModuleAccelerations[m_numModules]; if (chassisAccelerations.ax == 0.0 @@ -476,19 +490,31 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( if (!centerOfRotation.equals(m_prevCoR)) { for (int i = 0; i < m_numModules; i++) { - m_inverseKinematics.setRow( - i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotation.getY()); - m_inverseKinematics.setRow( - i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX() - centerOfRotation.getX()); + // Map chassis accelerations [ax, ay, ω², α] to module acceleration vector components [x, + // y]. + // We intentionally exclude the centripetal (−ω² r) contribution from module accelerations + // so + // modules receive only the tangential component (α × r) plus chassis [ax, ay]. This matches + // the expected behavior in tests that command tangential module accelerations for a given + // α. + var rx = m_modules[i].getX() - centerOfRotation.getX(); + var ry = m_modules[i].getY() - centerOfRotation.getY(); + m_secondOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */1, 0, -ry, -rx); + m_secondOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, -rx, +ry); } m_prevCoR = centerOfRotation; } - var chassisAccelerationsVector = new SimpleMatrix(3, 1); + var chassisAccelerationsVector = new SimpleMatrix(4, 1); chassisAccelerationsVector.setColumn( - 0, 0, chassisAccelerations.ax, chassisAccelerations.ay, chassisAccelerations.alpha); + 0, + 0, + chassisAccelerations.ax, + chassisAccelerations.ay, + Math.pow(angularVelocity, 2), + chassisAccelerations.alpha); - var moduleAccelerationsMatrix = m_inverseKinematics.mult(chassisAccelerationsVector); + var moduleAccelerationsMatrix = m_secondOrderInverseKinematics.mult(chassisAccelerationsVector); for (int i = 0; i < m_numModules; i++) { double x = moduleAccelerationsMatrix.get(i * 2, 0); @@ -499,7 +525,8 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( double linearAcceleration = Math.hypot(x, y); moduleAccelerations[i] = - new SwerveModuleAccelerations(linearAcceleration, Rotation2d.fromRadians(Math.atan2(y, x))); + new SwerveModuleAccelerations( + linearAcceleration, Rotation2d.fromRadians(Math.atan2(y, x))); } return moduleAccelerations; @@ -507,20 +534,21 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( /** * Performs inverse kinematics. See {@link #toSwerveModuleAccelerations(ChassisAccelerations, - * Translation2d)} toSwerveModuleAccelerations for more information. + * double, Translation2d)} toSwerveModuleAccelerations for more information. * * @param chassisAccelerations The desired chassis accelerations. + * @param angularVelocity The desired robot angular velocity. * @return An array containing the module accelerations. */ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( - ChassisAccelerations chassisAccelerations) { - return toSwerveModuleAccelerations(chassisAccelerations, Translation2d.kZero); + ChassisAccelerations chassisAccelerations, double angularVelocity) { + return toSwerveModuleAccelerations(chassisAccelerations, angularVelocity, Translation2d.kZero); } @Override public SwerveModuleAccelerations[] toWheelAccelerations( ChassisAccelerations chassisAccelerations) { - return toSwerveModuleAccelerations(chassisAccelerations); + return toSwerveModuleAccelerations(chassisAccelerations, 0.0); } /** @@ -547,14 +575,16 @@ public ChassisAccelerations toChassisAccelerations( for (int i = 0; i < m_numModules; i++) { var module = moduleAccelerations[i]; - moduleAccelerationsMatrix.set(i * 2, 0, module.acceleration * module.angle.getCos()); - moduleAccelerationsMatrix.set(i * 2 + 1, module.acceleration * module.angle.getSin()); + moduleAccelerationsMatrix.set(i * 2 + 0, 0, module.acceleration * module.angle.getCos()); + moduleAccelerationsMatrix.set(i * 2 + 1, 0, module.acceleration * module.angle.getSin()); } - var chassisAccelerationsVector = m_forwardKinematics.mult(moduleAccelerationsMatrix); + var chassisAccelerationsVector = m_secondOrderForwardKinematics.mult(moduleAccelerationsMatrix); + + // the second order kinematics equation for swerve drive yields a state vector [aₓ, a_y, ω², α] return new ChassisAccelerations( chassisAccelerationsVector.get(0, 0), chassisAccelerationsVector.get(1, 0), - chassisAccelerationsVector.get(2, 0)); + chassisAccelerationsVector.get(3, 0)); } } From 5d4d723b1811cc90cdbab5ac5fd611af3c7e338d Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 22 Aug 2025 10:47:59 -0400 Subject: [PATCH 21/82] refactor: update SwerveDriveKinematicsTest to include angular velocity in module acceleration calculations Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematicsTest.java | 75 +++++++++++-------- 1 file changed, 43 insertions(+), 32 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 12a626de8b4..0159be8dc0f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -410,29 +410,33 @@ void testDesaturateNegativeSpeed() { @Test void testTurnInPlaceInverseAccelerations() { ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 2 * Math.PI); - var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations); + double angularVelocity = 2 * Math.PI; + var moduleAccelerations = + m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity); /* - The circumference of the wheels about the COR is π * diameter, or 2π * radius - the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels - trace out is 106.629190516in. - since we want our robot to rotate at 1 rotation per second squared, - our wheels must trace out 1 rotation (or 106.63 inches) per second squared. + For turn-in-place with angular acceleration of 2π rad/s² and angular velocity of 2π rad/s, + each module has both tangential acceleration (from angular acceleration) and centripetal + acceleration (from angular velocity). The total acceleration magnitude is approximately 678.4. - For turn-in-place, each module accelerates tangentially to the circle, so the angle - of acceleration should match the expected turning angles: - FL: 135°, FR: 45°, BL: -135°, BR: -45° - */ + The acceleration angles are not simply tangential (as in pure rotation) because the + centripetal component from the existing angular velocity affects the direction: + FL: -144°, FR: 126°, BL: -54°, BR: 36° + + These angles reflect the combination of: + - Tangential acceleration from angular acceleration (2π rad/s²) + - Centripetal acceleration from angular velocity (2π rad/s) + */ assertAll( - () -> assertEquals(106.63, moduleAccelerations[0].acceleration, 0.1), - () -> assertEquals(106.63, moduleAccelerations[1].acceleration, 0.1), - () -> assertEquals(106.63, moduleAccelerations[2].acceleration, 0.1), - () -> assertEquals(106.63, moduleAccelerations[3].acceleration, 0.1), - () -> assertEquals(135.0, moduleAccelerations[0].angle.getDegrees(), 0.1), - () -> assertEquals(45.0, moduleAccelerations[1].angle.getDegrees(), 0.1), - () -> assertEquals(-135.0, moduleAccelerations[2].angle.getDegrees(), 0.1), - () -> assertEquals(-45.0, moduleAccelerations[3].angle.getDegrees(), 0.1)); + () -> assertEquals(678.4, moduleAccelerations[0].acceleration, 0.1), + () -> assertEquals(678.4, moduleAccelerations[1].acceleration, 0.1), + () -> assertEquals(678.4, moduleAccelerations[2].acceleration, 0.1), + () -> assertEquals(678.4, moduleAccelerations[3].acceleration, 0.1), + () -> assertEquals(-144.0, moduleAccelerations[0].angle.getDegrees(), 0.1), + () -> assertEquals(126.0, moduleAccelerations[1].angle.getDegrees(), 0.1), + () -> assertEquals(-54.0, moduleAccelerations[2].angle.getDegrees(), 0.1), + () -> assertEquals(36.0, moduleAccelerations[3].angle.getDegrees(), 0.1)); } @Test @@ -458,25 +462,32 @@ void testTurnInPlaceForwardAccelerations() { @Test void testOffCenterRotationInverseAccelerations() { ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 1); - var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations, m_fl); + // For this test, assume an angular velocity of 1 rad/s + double angularVelocity = 1.0; + var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity, m_fl); /* - When rotating about the front-left module position, each module accelerates - tangentially to its circular path. The angles should be: - FL: stationary (0 acceleration) - FR: straight forward (0°) - BL: straight right (-90°) - BR: -45° (tangent to the diagonal path) + When rotating about the front-left module position with both angular acceleration (1 rad/s²) + and angular velocity (1 rad/s), each module experiences both tangential and centripetal + accelerations that combine vectorially. + + FL: at center of rotation (0 acceleration) + FR: 24 units from center, experiences combined acceleration → ~33.94 + BL: 24 units from center, experiences combined acceleration → ~33.94 + BR: ~33.94 units from center, experiences combined acceleration → ~48.0 + + Angles reflect the vector combination of tangential and centripetal components: + FL: -45° (though magnitude is 0), FR: -45°, BL: 45°, BR: 0° */ assertAll( () -> assertEquals(0, moduleAccelerations[0].acceleration, 0.1), - () -> assertEquals(24.0, moduleAccelerations[1].acceleration, 0.1), - () -> assertEquals(24.0, moduleAccelerations[2].acceleration, 0.1), - () -> assertEquals(33.941, moduleAccelerations[3].acceleration, 0.1), - () -> assertEquals(0.0, moduleAccelerations[0].angle.getDegrees(), 0.1), // FL stationary - () -> assertEquals(0.0, moduleAccelerations[1].angle.getDegrees(), 0.1), // FR forward - () -> assertEquals(-90.0, moduleAccelerations[2].angle.getDegrees(), 0.1), // BL right - () -> assertEquals(-45.0, moduleAccelerations[3].angle.getDegrees(), 0.1)); // BR diagonal + () -> assertEquals(33.94, moduleAccelerations[1].acceleration, 0.1), + () -> assertEquals(33.94, moduleAccelerations[2].acceleration, 0.1), + () -> assertEquals(48.0, moduleAccelerations[3].acceleration, 0.1), + () -> assertEquals(0.0, moduleAccelerations[0].angle.getDegrees(), 0.1), + () -> assertEquals(-45.0, moduleAccelerations[1].angle.getDegrees(), 0.1), + () -> assertEquals(45.0, moduleAccelerations[2].angle.getDegrees(), 0.1), + () -> assertEquals(0.0, moduleAccelerations[3].angle.getDegrees(), 0.1)); } } From 5db0a7899092db78374ffbee9819cb15576515fa Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 22 Aug 2025 20:53:53 -0400 Subject: [PATCH 22/82] refactor: extract the setting of inverse kinematics matrix in swervedrivekinematics to its own method Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematics.java | 62 ++++++++----------- .../kinematics/SwerveDriveKinematicsTest.java | 9 +-- 2 files changed, 32 insertions(+), 39 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index 0e5bae3371c..e55e318a26c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -77,17 +77,8 @@ public SwerveDriveKinematics(Translation2d... moduleTranslations) { m_firstOrderInverseKinematics = new SimpleMatrix(m_numModules * 2, 3); m_secondOrderInverseKinematics = new SimpleMatrix(m_numModules * 2, 4); - for (int i = 0; i < m_numModules; i++) { - m_firstOrderInverseKinematics.setRow( - i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY()); - m_firstOrderInverseKinematics.setRow( - i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX()); - - m_secondOrderInverseKinematics.setRow( - i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY()); - m_secondOrderInverseKinematics.setRow( - i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX()); - } + setInverseKinematics(Translation2d.kZero); + m_firstOrderForwardKinematics = m_firstOrderInverseKinematics.pseudoInverse(); m_secondOrderForwardKinematics = m_secondOrderInverseKinematics.pseudoInverse(); @@ -143,13 +134,7 @@ public SwerveModuleState[] toSwerveModuleStates( } if (!centerOfRotation.equals(m_prevCoR)) { - for (int i = 0; i < m_numModules; i++) { - m_firstOrderInverseKinematics.setRow( - i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotation.getY()); - m_firstOrderInverseKinematics.setRow( - i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX() - centerOfRotation.getX()); - } - m_prevCoR = centerOfRotation; + setInverseKinematics(centerOfRotation); } var chassisSpeedsVector = new SimpleMatrix(3, 1); @@ -489,20 +474,7 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( } if (!centerOfRotation.equals(m_prevCoR)) { - for (int i = 0; i < m_numModules; i++) { - // Map chassis accelerations [ax, ay, ω², α] to module acceleration vector components [x, - // y]. - // We intentionally exclude the centripetal (−ω² r) contribution from module accelerations - // so - // modules receive only the tangential component (α × r) plus chassis [ax, ay]. This matches - // the expected behavior in tests that command tangential module accelerations for a given - // α. - var rx = m_modules[i].getX() - centerOfRotation.getX(); - var ry = m_modules[i].getY() - centerOfRotation.getY(); - m_secondOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */1, 0, -ry, -rx); - m_secondOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, -rx, +ry); - } - m_prevCoR = centerOfRotation; + setInverseKinematics(centerOfRotation); } var chassisAccelerationsVector = new SimpleMatrix(4, 1); @@ -511,7 +483,7 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( 0, chassisAccelerations.ax, chassisAccelerations.ay, - Math.pow(angularVelocity, 2), + angularVelocity * angularVelocity, chassisAccelerations.alpha); var moduleAccelerationsMatrix = m_secondOrderInverseKinematics.mult(chassisAccelerationsVector); @@ -525,8 +497,7 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( double linearAcceleration = Math.hypot(x, y); moduleAccelerations[i] = - new SwerveModuleAccelerations( - linearAcceleration, Rotation2d.fromRadians(Math.atan2(y, x))); + new SwerveModuleAccelerations(linearAcceleration, new Rotation2d(x, y)); } return moduleAccelerations; @@ -587,4 +558,25 @@ public ChassisAccelerations toChassisAccelerations( chassisAccelerationsVector.get(1, 0), chassisAccelerationsVector.get(3, 0)); } + + /** + * Sets both inverse kinematics matrices based on the new center of rotation. This does not check + * if the new center of rotation is different from the previous one, so a check should be included + * before the call to this function. + * + * @param centerOfRotation new center of rotation + */ + private void setInverseKinematics(Translation2d centerOfRotation) { + for (int i = 0; i < m_numModules; i++) { + var rx = m_modules[i].getX() - centerOfRotation.getX(); + var ry = m_modules[i].getY() - centerOfRotation.getY(); + + m_firstOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -ry); + m_firstOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, rx); + + m_secondOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -rx, -ry); + m_secondOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, -ry, +rx); + } + m_prevCoR = centerOfRotation; + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 0159be8dc0f..724704fc7ab 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -464,7 +464,8 @@ void testOffCenterRotationInverseAccelerations() { ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 1); // For this test, assume an angular velocity of 1 rad/s double angularVelocity = 1.0; - var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity, m_fl); + var moduleAccelerations = + m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity, m_fl); /* When rotating about the front-left module position with both angular acceleration (1 rad/s²) @@ -477,7 +478,7 @@ and angular velocity (1 rad/s), each module experiences both tangential and cent BR: ~33.94 units from center, experiences combined acceleration → ~48.0 Angles reflect the vector combination of tangential and centripetal components: - FL: -45° (though magnitude is 0), FR: -45°, BL: 45°, BR: 0° + FL: -45° (though magnitude is 0), FR: 45°, BL: -45°, BR: 0° */ assertAll( @@ -486,8 +487,8 @@ and angular velocity (1 rad/s), each module experiences both tangential and cent () -> assertEquals(33.94, moduleAccelerations[2].acceleration, 0.1), () -> assertEquals(48.0, moduleAccelerations[3].acceleration, 0.1), () -> assertEquals(0.0, moduleAccelerations[0].angle.getDegrees(), 0.1), - () -> assertEquals(-45.0, moduleAccelerations[1].angle.getDegrees(), 0.1), - () -> assertEquals(45.0, moduleAccelerations[2].angle.getDegrees(), 0.1), + () -> assertEquals(45.0, moduleAccelerations[1].angle.getDegrees(), 0.1), + () -> assertEquals(-45.0, moduleAccelerations[2].angle.getDegrees(), 0.1), () -> assertEquals(0.0, moduleAccelerations[3].angle.getDegrees(), 0.1)); } } From adaeffd75bf683771b1e4f0dd900cf2d413ae0dd Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 22 Aug 2025 20:54:05 -0400 Subject: [PATCH 23/82] refactor: update ChassisAccelerations constructor parameter descriptions for clarity Signed-off-by: Zach Harel --- .../edu/wpi/first/math/kinematics/ChassisAccelerations.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java index fb86debe5bc..eef24c03b2e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java @@ -64,9 +64,9 @@ public ChassisAccelerations(double ax, double ay, double alpha) { /** * Constructs a ChassisAccelerations object. * - * @param ax Forward velocity. - * @param ay Sideways velocity. - * @param alpha Angular velocity. + * @param ax Forward acceleration. + * @param ay Sideways acceleration. + * @param alpha Angular acceleration. */ public ChassisAccelerations( LinearAcceleration ax, LinearAcceleration ay, AngularAcceleration alpha) { From e1170eec26279c0d9b59a67d07af65965663ed10 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 22 Aug 2025 20:54:41 -0400 Subject: [PATCH 24/82] refactor: update SwerveModuleAccelerations struct and protobuf to use Rotation2d/ProtobufRotation2d for angle representation instead of a double Signed-off-by: Zach Harel --- .../edu/wpi/first/math/proto/Kinematics.java | 157 +++--- .../cpp/wpimath/protobuf/kinematics.npb.cpp | 495 +++++++++--------- .../cpp/wpimath/protobuf/kinematics.npb.h | 13 +- .../proto/SwerveModuleAccelerationsProto.java | 5 +- .../SwerveModuleAccelerationsStruct.java | 10 +- .../proto/SwerveModuleAccelerationsProto.cpp | 35 +- .../SwerveModuleAccelerationsStruct.cpp | 5 +- .../struct/SwerveModuleAccelerationsStruct.h | 4 +- wpimath/src/main/proto/kinematics.proto | 2 +- 9 files changed, 382 insertions(+), 344 deletions(-) diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java index cd093333e70..0d454db07ed 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java @@ -19,7 +19,7 @@ import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4158, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4189, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + "BW9tZWdhIlQKHFByb3RvYnVmQ2hhc3Npc0FjY2VsZXJhdGlvbnMSDgoCYXgYASABKAFSAmF4Eg4KAmF5" + @@ -47,49 +47,49 @@ public final class Kinematics { "ZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5w" + "cm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUS" + "FAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90" + - "YXRpb24yZFIFYW5nbGUiXQohUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb25zEiIKDGFjY2Vs" + - "ZXJhdGlvbhgBIAEoAVIMYWNjZWxlcmF0aW9uEhQKBWFuZ2xlGAIgASgBUgVhbmdsZUIaChhlZHUud3Bp" + - "LmZpcnN0Lm1hdGgucHJvdG9K1hIKBhIEAABUAQoICgEMEgMAABIKCAoBAhIDAgASCgkKAgMAEgMEABoK" + - "CAoBCBIDBgAxCgkKAggBEgMGADEKCgoCBAASBAgADAEKCgoDBAABEgMICB0KCwoEBAACABIDCQIQCgwK" + - "BQQAAgAFEgMJAggKDAoFBAACAAESAwkJCwoMCgUEAAIAAxIDCQ4PCgsKBAQAAgESAwoCEAoMCgUEAAIB" + - "BRIDCgIICgwKBQQAAgEBEgMKCQsKDAoFBAACAQMSAwoODwoLCgQEAAICEgMLAhMKDAoFBAACAgUSAwsC" + - "CAoMCgUEAAICARIDCwkOCgwKBQQAAgIDEgMLERIKCgoCBAESBA4AEgEKCgoDBAEBEgMOCCQKCwoEBAEC" + - "ABIDDwIQCgwKBQQBAgAFEgMPAggKDAoFBAECAAESAw8JCwoMCgUEAQIAAxIDDw4PCgsKBAQBAgESAxAC" + - "EAoMCgUEAQIBBRIDEAIICgwKBQQBAgEBEgMQCQsKDAoFBAECAQMSAxAODwoLCgQEAQICEgMRAhMKDAoF" + - "BAECAgUSAxECCAoMCgUEAQICARIDEQkOCgwKBQQBAgIDEgMRERIKCgoCBAISBBQAFgEKCgoDBAIBEgMU" + - "CCsKCwoEBAICABIDFQIYCgwKBQQCAgAFEgMVAggKDAoFBAICAAESAxUJEwoMCgUEAgIAAxIDFRYXCgoK" + - "AgQDEgQYABsBCgoKAwQDARIDGAgsCgsKBAQDAgASAxkCEgoMCgUEAwIABRIDGQIICgwKBQQDAgABEgMZ" + - "CQ0KDAoFBAMCAAMSAxkQEQoLCgQEAwIBEgMaAhMKDAoFBAMCAQUSAxoCCAoMCgUEAwIBARIDGgkOCgwK", - "BQQDAgEDEgMaERIKCgoCBAQSBB0AIAEKCgoDBAQBEgMdCDMKCwoEBAQCABIDHgISCgwKBQQEAgAFEgMe" + - "AggKDAoFBAQCAAESAx4JDQoMCgUEBAIAAxIDHhARCgsKBAQEAgESAx8CEwoMCgUEBAIBBRIDHwIICgwK" + - "BQQEAgEBEgMfCQ4KDAoFBAQCAQMSAx8REgoKCgIEBRIEIgAlAQoKCgMEBQESAyIILwoLCgQEBQIAEgMj" + - "AhIKDAoFBAUCAAUSAyMCCAoMCgUEBQIAARIDIwkNCgwKBQQFAgADEgMjEBEKCwoEBAUCARIDJAITCgwK" + - "BQQFAgEFEgMkAggKDAoFBAUCAQESAyQJDgoMCgUEBQIBAxIDJBESCgoKAgQGEgQnACwBCgoKAwQGARID" + - "JwgmCgsKBAQGAgASAygCJwoMCgUEBgIABhIDKAIXCgwKBQQGAgABEgMoGCIKDAoFBAYCAAMSAyglJgoL" + - "CgQEBgIBEgMpAigKDAoFBAYCAQYSAykCFwoMCgUEBgIBARIDKRgjCgwKBQQGAgEDEgMpJicKCwoEBAYC" + - "AhIDKgImCgwKBQQGAgIGEgMqAhcKDAoFBAYCAgESAyoYIQoMCgUEBgICAxIDKiQlCgsKBAQGAgMSAysC" + - "JwoMCgUEBgIDBhIDKwIXCgwKBQQGAgMBEgMrGCIKDAoFBAYCAwMSAyslJgoKCgIEBxIELgAzAQoKCgME" + - "BwESAy4IKgoLCgQEBwIAEgMvAhgKDAoFBAcCAAUSAy8CCAoMCgUEBwIAARIDLwkTCgwKBQQHAgADEgMv" + - "FhcKCwoEBAcCARIDMAIZCgwKBQQHAgEFEgMwAggKDAoFBAcCAQESAzAJFAoMCgUEBwIBAxIDMBcYCgsK" + - "BAQHAgISAzECFwoMCgUEBwICBRIDMQIICgwKBQQHAgIBEgMxCRIKDAoFBAcCAgMSAzEVFgoLCgQEBwID" + - "EgMyAhgKDAoFBAcCAwUSAzICCAoMCgUEBwIDARIDMgkTCgwKBQQHAgMDEgMyFhcKCgoCBAgSBDUAOgEK" + - "CgoDBAgBEgM1CCcKCwoEBAgCABIDNgIYCgwKBQQIAgAFEgM2AggKDAoFBAgCAAESAzYJEwoMCgUECAIA" + - "AxIDNhYXCgsKBAQIAgESAzcCGQoMCgUECAIBBRIDNwIICgwKBQQIAgEBEgM3CRQKDAoFBAgCAQMSAzcX" + - "GAoLCgQECAICEgM4AhcKDAoFBAgCAgUSAzgCCAoMCgUECAICARIDOAkSCgwKBQQIAgIDEgM4FRYKCwoE" + - "BAgCAxIDOQIYCgwKBQQIAgMFEgM5AggKDAoFBAgCAwESAzkJEwoMCgUECAIDAxIDORYXCgoKAgQJEgQ8" + - "AEEBCgoKAwQJARIDPAguCgsKBAQJAgASAz0CGAoMCgUECQIABRIDPQIICgwKBQQJAgABEgM9CRMKDAoF" + - "BAkCAAMSAz0WFwoLCgQECQIBEgM+AhkKDAoFBAkCAQUSAz4CCAoMCgUECQIBARIDPgkUCgwKBQQJAgED" + - "EgM+FxgKCwoEBAkCAhIDPwIXCgwKBQQJAgIFEgM/AggKDAoFBAkCAgESAz8JEgoMCgUECQICAxIDPxUW", - "CgsKBAQJAgMSA0ACGAoMCgUECQIDBRIDQAIICgwKBQQJAgMBEgNACRMKDAoFBAkCAwMSA0AWFwoKCgIE" + - "ChIEQwBFAQoKCgMECgESA0MIJQoLCgQECgIAEgNEAi0KDAoFBAoCAAQSA0QCCgoMCgUECgIABhIDRAsg" + - "CgwKBQQKAgABEgNEISgKDAoFBAoCAAMSA0QrLAoKCgIECxIERwBKAQoKCgMECwESA0cIJAoLCgQECwIA" + - "EgNIAhYKDAoFBAsCAAUSA0gCCAoMCgUECwIAARIDSAkRCgwKBQQLAgADEgNIFBUKCwoEBAsCARIDSQIf" + - "CgwKBQQLAgEGEgNJAhQKDAoFBAsCAQESA0kVGgoMCgUECwIBAxIDSR0eCgoKAgQMEgRMAE8BCgoKAwQM" + - "ARIDTAghCgsKBAQMAgASA00CEwoMCgUEDAIABRIDTQIICgwKBQQMAgABEgNNCQ4KDAoFBAwCAAMSA00R" + - "EgoLCgQEDAIBEgNOAh8KDAoFBAwCAQYSA04CFAoMCgUEDAIBARIDThUaCgwKBQQMAgEDEgNOHR4KCgoC" + - "BA0SBFEAVAEKCgoDBA0BEgNRCCkKCwoEBA0CABIDUgIaCgwKBQQNAgAFEgNSAggKDAoFBA0CAAESA1IJ" + - "FQoMCgUEDQIAAxIDUhgZCgsKBAQNAgESA1MCEwoMCgUEDQIBBRIDUwIICgwKBQQNAgEBEgNTCQ4KDAoF" + - "BA0CAQMSA1MREmIGcHJvdG8z"); + "YXRpb24yZFIFYW5nbGUifAohUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb25zEiIKDGFjY2Vs" + + "ZXJhdGlvbhgBIAEoAVIMYWNjZWxlcmF0aW9uEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3Rv" + + "YnVmUm90YXRpb24yZFIFYW5nbGVCGgoYZWR1LndwaS5maXJzdC5tYXRoLnByb3RvStYSCgYSBAAAVAEK" + + "CAoBDBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAaCggKAQgSAwYAMQoJCgIIARIDBgAxCgoKAgQAEgQI" + + "AAwBCgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoMCgUEAAIABRIDCQIICgwKBQQAAgABEgMJCQsKDAoF" + + "BAACAAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAACAQUSAwoCCAoMCgUEAAIBARIDCgkLCgwKBQQAAgED" + + "EgMKDg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgMLAggKDAoFBAACAgESAwsJDgoMCgUEAAICAxIDCxES" + + "CgoKAgQBEgQOABIBCgoKAwQBARIDDggkCgsKBAQBAgASAw8CEAoMCgUEAQIABRIDDwIICgwKBQQBAgAB" + + "EgMPCQsKDAoFBAECAAMSAw8ODwoLCgQEAQIBEgMQAhAKDAoFBAECAQUSAxACCAoMCgUEAQIBARIDEAkL" + + "CgwKBQQBAgEDEgMQDg8KCwoEBAECAhIDEQITCgwKBQQBAgIFEgMRAggKDAoFBAECAgESAxEJDgoMCgUE" + + "AQICAxIDERESCgoKAgQCEgQUABYBCgoKAwQCARIDFAgrCgsKBAQCAgASAxUCGAoMCgUEAgIABRIDFQII" + + "CgwKBQQCAgABEgMVCRMKDAoFBAICAAMSAxUWFwoKCgIEAxIEGAAbAQoKCgMEAwESAxgILAoLCgQEAwIA" + + "EgMZAhIKDAoFBAMCAAUSAxkCCAoMCgUEAwIAARIDGQkNCgwKBQQDAgADEgMZEBEKCwoEBAMCARIDGgIT", + "CgwKBQQDAgEFEgMaAggKDAoFBAMCAQESAxoJDgoMCgUEAwIBAxIDGhESCgoKAgQEEgQdACABCgoKAwQE" + + "ARIDHQgzCgsKBAQEAgASAx4CEgoMCgUEBAIABRIDHgIICgwKBQQEAgABEgMeCQ0KDAoFBAQCAAMSAx4Q" + + "EQoLCgQEBAIBEgMfAhMKDAoFBAQCAQUSAx8CCAoMCgUEBAIBARIDHwkOCgwKBQQEAgEDEgMfERIKCgoC" + + "BAUSBCIAJQEKCgoDBAUBEgMiCC8KCwoEBAUCABIDIwISCgwKBQQFAgAFEgMjAggKDAoFBAUCAAESAyMJ" + + "DQoMCgUEBQIAAxIDIxARCgsKBAQFAgESAyQCEwoMCgUEBQIBBRIDJAIICgwKBQQFAgEBEgMkCQ4KDAoF" + + "BAUCAQMSAyQREgoKCgIEBhIEJwAsAQoKCgMEBgESAycIJgoLCgQEBgIAEgMoAicKDAoFBAYCAAYSAygC" + + "FwoMCgUEBgIAARIDKBgiCgwKBQQGAgADEgMoJSYKCwoEBAYCARIDKQIoCgwKBQQGAgEGEgMpAhcKDAoF" + + "BAYCAQESAykYIwoMCgUEBgIBAxIDKSYnCgsKBAQGAgISAyoCJgoMCgUEBgICBhIDKgIXCgwKBQQGAgIB" + + "EgMqGCEKDAoFBAYCAgMSAyokJQoLCgQEBgIDEgMrAicKDAoFBAYCAwYSAysCFwoMCgUEBgIDARIDKxgi" + + "CgwKBQQGAgMDEgMrJSYKCgoCBAcSBC4AMwEKCgoDBAcBEgMuCCoKCwoEBAcCABIDLwIYCgwKBQQHAgAF" + + "EgMvAggKDAoFBAcCAAESAy8JEwoMCgUEBwIAAxIDLxYXCgsKBAQHAgESAzACGQoMCgUEBwIBBRIDMAII" + + "CgwKBQQHAgEBEgMwCRQKDAoFBAcCAQMSAzAXGAoLCgQEBwICEgMxAhcKDAoFBAcCAgUSAzECCAoMCgUE" + + "BwICARIDMQkSCgwKBQQHAgIDEgMxFRYKCwoEBAcCAxIDMgIYCgwKBQQHAgMFEgMyAggKDAoFBAcCAwES" + + "AzIJEwoMCgUEBwIDAxIDMhYXCgoKAgQIEgQ1ADoBCgoKAwQIARIDNQgnCgsKBAQIAgASAzYCGAoMCgUE" + + "CAIABRIDNgIICgwKBQQIAgABEgM2CRMKDAoFBAgCAAMSAzYWFwoLCgQECAIBEgM3AhkKDAoFBAgCAQUS" + + "AzcCCAoMCgUECAIBARIDNwkUCgwKBQQIAgEDEgM3FxgKCwoEBAgCAhIDOAIXCgwKBQQIAgIFEgM4AggK" + + "DAoFBAgCAgESAzgJEgoMCgUECAICAxIDOBUWCgsKBAQIAgMSAzkCGAoMCgUECAIDBRIDOQIICgwKBQQI" + + "AgMBEgM5CRMKDAoFBAgCAwMSAzkWFwoKCgIECRIEPABBAQoKCgMECQESAzwILgoLCgQECQIAEgM9AhgK" + + "DAoFBAkCAAUSAz0CCAoMCgUECQIAARIDPQkTCgwKBQQJAgADEgM9FhcKCwoEBAkCARIDPgIZCgwKBQQJ" + + "AgEFEgM+AggKDAoFBAkCAQESAz4JFAoMCgUECQIBAxIDPhcYCgsKBAQJAgISAz8CFwoMCgUECQICBRID", + "PwIICgwKBQQJAgIBEgM/CRIKDAoFBAkCAgMSAz8VFgoLCgQECQIDEgNAAhgKDAoFBAkCAwUSA0ACCAoM" + + "CgUECQIDARIDQAkTCgwKBQQJAgMDEgNAFhcKCgoCBAoSBEMARQEKCgoDBAoBEgNDCCUKCwoEBAoCABID" + + "RAItCgwKBQQKAgAEEgNEAgoKDAoFBAoCAAYSA0QLIAoMCgUECgIAARIDRCEoCgwKBQQKAgADEgNEKywK" + + "CgoCBAsSBEcASgEKCgoDBAsBEgNHCCQKCwoEBAsCABIDSAIWCgwKBQQLAgAFEgNIAggKDAoFBAsCAAES" + + "A0gJEQoMCgUECwIAAxIDSBQVCgsKBAQLAgESA0kCHwoMCgUECwIBBhIDSQIUCgwKBQQLAgEBEgNJFRoK" + + "DAoFBAsCAQMSA0kdHgoKCgIEDBIETABPAQoKCgMEDAESA0wIIQoLCgQEDAIAEgNNAhMKDAoFBAwCAAUS" + + "A00CCAoMCgUEDAIAARIDTQkOCgwKBQQMAgADEgNNERIKCwoEBAwCARIDTgIfCgwKBQQMAgEGEgNOAhQK" + + "DAoFBAwCAQESA04VGgoMCgUEDAIBAxIDTh0eCgoKAgQNEgRRAFQBCgoKAwQNARIDUQgpCgsKBAQNAgAS" + + "A1ICGgoMCgUEDQIABRIDUgIICgwKBQQNAgABEgNSCRUKDAoFBA0CAAMSA1IYGQoLCgQEDQIBEgNTAh8K" + + "DAoFBA0CAQYSA1MCFAoMCgUEDQIBARIDUxUaCgwKBQQNAgEDEgNTHR5iBnByb3RvMw=="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); @@ -119,7 +119,7 @@ public final class Kinematics { static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1532, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleAccelerations_descriptor = descriptor.internalContainedType(1636, 93, "ProtobufSwerveModuleAccelerations", "wpi.proto.ProtobufSwerveModuleAccelerations"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleAccelerations_descriptor = descriptor.internalContainedType(1636, 124, "ProtobufSwerveModuleAccelerations", "wpi.proto.ProtobufSwerveModuleAccelerations"); /** * @return this proto file's descriptor. @@ -5321,9 +5321,9 @@ public static final class ProtobufSwerveModuleAccelerations extends ProtoMessage private double acceleration; /** - * optional double angle = 2; + * optional .wpi.proto.ProtobufRotation2d angle = 2; */ - private double angle; + private final Geometry2D.ProtobufRotation2d angle = Geometry2D.ProtobufRotation2d.newInstance(); private ProtobufSwerveModuleAccelerations() { } @@ -5373,7 +5373,7 @@ public ProtobufSwerveModuleAccelerations setAcceleration(final double value) { } /** - * optional double angle = 2; + * optional .wpi.proto.ProtobufRotation2d angle = 2; * @return whether the angle field is set */ public boolean hasAngle() { @@ -5381,31 +5381,51 @@ public boolean hasAngle() { } /** - * optional double angle = 2; + * optional .wpi.proto.ProtobufRotation2d angle = 2; * @return this */ public ProtobufSwerveModuleAccelerations clearAngle() { bitField0_ &= ~0x00000002; - angle = 0D; + angle.clear(); return this; } /** - * optional double angle = 2; - * @return the angle + * optional .wpi.proto.ProtobufRotation2d angle = 2; + * + * This method returns the internal storage object without modifying any has state. + * The returned object should not be modified and be treated as read-only. + * + * Use {@link #getMutableAngle()} if you want to modify it. + * + * @return internal storage object for reading */ - public double getAngle() { + public Geometry2D.ProtobufRotation2d getAngle() { return angle; } /** - * optional double angle = 2; + * optional .wpi.proto.ProtobufRotation2d angle = 2; + * + * This method returns the internal storage object and sets the corresponding + * has state. The returned object will become part of this message and its + * contents may be modified as long as the has state is not cleared. + * + * @return internal storage object for modifications + */ + public Geometry2D.ProtobufRotation2d getMutableAngle() { + bitField0_ |= 0x00000002; + return angle; + } + + /** + * optional .wpi.proto.ProtobufRotation2d angle = 2; * @param value the angle to set * @return this */ - public ProtobufSwerveModuleAccelerations setAngle(final double value) { + public ProtobufSwerveModuleAccelerations setAngle(final Geometry2D.ProtobufRotation2d value) { bitField0_ |= 0x00000002; - angle = value; + angle.copyFrom(value); return this; } @@ -5416,7 +5436,7 @@ public ProtobufSwerveModuleAccelerations copyFrom( if ((bitField0_ | other.bitField0_) != 0) { bitField0_ = other.bitField0_; acceleration = other.acceleration; - angle = other.angle; + angle.copyFrom(other.angle); } return this; } @@ -5432,7 +5452,7 @@ public ProtobufSwerveModuleAccelerations mergeFrom( setAcceleration(other.acceleration); } if (other.hasAngle()) { - setAngle(other.angle); + getMutableAngle().mergeFrom(other.angle); } return this; } @@ -5445,7 +5465,7 @@ public ProtobufSwerveModuleAccelerations clear() { cachedSize = -1; bitField0_ = 0; acceleration = 0D; - angle = 0D; + angle.clear(); return this; } @@ -5456,6 +5476,7 @@ public ProtobufSwerveModuleAccelerations clearQuick() { } cachedSize = -1; bitField0_ = 0; + angle.clearQuick(); return this; } @@ -5470,7 +5491,7 @@ public boolean equals(Object o) { ProtobufSwerveModuleAccelerations other = (ProtobufSwerveModuleAccelerations) o; return bitField0_ == other.bitField0_ && (!hasAcceleration() || ProtoUtil.isEqual(acceleration, other.acceleration)) - && (!hasAngle() || ProtoUtil.isEqual(angle, other.angle)); + && (!hasAngle() || angle.equals(other.angle)); } @Override @@ -5480,8 +5501,8 @@ public void writeTo(final ProtoSink output) throws IOException { output.writeDoubleNoTag(acceleration); } if ((bitField0_ & 0x00000002) != 0) { - output.writeRawByte((byte) 17); - output.writeDoubleNoTag(angle); + output.writeRawByte((byte) 18); + output.writeMessageNoTag(angle); } } @@ -5492,7 +5513,7 @@ protected int computeSerializedSize() { size += 9; } if ((bitField0_ & 0x00000002) != 0) { - size += 9; + size += 1 + ProtoSink.computeMessageSizeNoTag(angle); } return size; } @@ -5509,13 +5530,13 @@ public ProtobufSwerveModuleAccelerations mergeFrom(final ProtoSource input) thro acceleration = input.readDouble(); bitField0_ |= 0x00000001; tag = input.readTag(); - if (tag != 17) { + if (tag != 18) { break; } } - case 17: { + case 18: { // angle - angle = input.readDouble(); + input.readMessage(angle); bitField0_ |= 0x00000002; tag = input.readTag(); if (tag != 0) { @@ -5543,7 +5564,7 @@ public void writeTo(final JsonSink output) throws IOException { output.writeDouble(FieldNames.acceleration, acceleration); } if ((bitField0_ & 0x00000002) != 0) { - output.writeDouble(FieldNames.angle, angle); + output.writeMessage(FieldNames.angle, angle); } output.endObject(); } @@ -5569,7 +5590,7 @@ public ProtobufSwerveModuleAccelerations mergeFrom(final JsonSource input) throw case 92960979: { if (input.isAtField(FieldNames.angle)) { if (!input.trySkipNullValue()) { - angle = input.readDouble(); + input.readMessage(angle); bitField0_ |= 0x00000002; } } else { diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp index 57e60de84a2..e2fcee2c043 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp @@ -175,259 +175,262 @@ static const uint8_t file_descriptor[] { 0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50, 0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74, 0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61, -0x6e,0x67,0x6c,0x65,0x22,0x5d,0x0a,0x21,0x50,0x72, +0x6e,0x67,0x6c,0x65,0x22,0x7c,0x0a,0x21,0x50,0x72, 0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72, 0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65,0x41,0x63, 0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,0x6e, 0x73,0x12,0x22,0x0a,0x0c,0x61,0x63,0x63,0x65,0x6c, 0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x18,0x01,0x20, 0x01,0x28,0x01,0x52,0x0c,0x61,0x63,0x63,0x65,0x6c, -0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x12,0x14,0x0a, +0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x12,0x33,0x0a, 0x05,0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01, -0x28,0x01,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65,0x42, -0x1a,0x0a,0x18,0x65,0x64,0x75,0x2e,0x77,0x70,0x69, -0x2e,0x66,0x69,0x72,0x73,0x74,0x2e,0x6d,0x61,0x74, -0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0xd6,0x12, -0x0a,0x06,0x12,0x04,0x00,0x00,0x54,0x01,0x0a,0x08, -0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08, -0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x09, -0x0a,0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x1a,0x0a, -0x08,0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x31,0x0a, -0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00,0x31, -0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x08,0x00, -0x0c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12, -0x03,0x08,0x08,0x1d,0x0a,0x0b,0x0a,0x04,0x04,0x00, -0x02,0x00,0x12,0x03,0x09,0x02,0x10,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x09,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01, -0x12,0x03,0x09,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x00,0x03,0x12,0x03,0x09,0x0e,0x0f,0x0a, -0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x0a, -0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01, -0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0a,0x09,0x0b, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12, -0x03,0x0a,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00, -0x02,0x02,0x12,0x03,0x0b,0x02,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x0b,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01, -0x12,0x03,0x0b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x02,0x03,0x12,0x03,0x0b,0x11,0x12,0x0a, -0x0a,0x0a,0x02,0x04,0x01,0x12,0x04,0x0e,0x00,0x12, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,0x03, -0x0e,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02, -0x00,0x12,0x03,0x0f,0x02,0x10,0x0a,0x0c,0x0a,0x05, -0x04,0x01,0x02,0x00,0x05,0x12,0x03,0x0f,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x01,0x12, -0x03,0x0f,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x01, -0x02,0x00,0x03,0x12,0x03,0x0f,0x0e,0x0f,0x0a,0x0b, -0x0a,0x04,0x04,0x01,0x02,0x01,0x12,0x03,0x10,0x02, -0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x05, -0x12,0x03,0x10,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x01,0x02,0x01,0x01,0x12,0x03,0x10,0x09,0x0b,0x0a, -0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x03,0x12,0x03, -0x10,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02, -0x02,0x12,0x03,0x11,0x02,0x13,0x0a,0x0c,0x0a,0x05, -0x04,0x01,0x02,0x02,0x05,0x12,0x03,0x11,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x01,0x12, -0x03,0x11,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x01, -0x02,0x02,0x03,0x12,0x03,0x11,0x11,0x12,0x0a,0x0a, -0x0a,0x02,0x04,0x02,0x12,0x04,0x14,0x00,0x16,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01,0x12,0x03,0x14, -0x08,0x2b,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x00, -0x12,0x03,0x15,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, -0x02,0x02,0x00,0x05,0x12,0x03,0x15,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x01,0x12,0x03, -0x15,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, -0x00,0x03,0x12,0x03,0x15,0x16,0x17,0x0a,0x0a,0x0a, -0x02,0x04,0x03,0x12,0x04,0x18,0x00,0x1b,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x18,0x08, -0x2c,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12, -0x03,0x19,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03, -0x02,0x00,0x05,0x12,0x03,0x19,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x19, -0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00, -0x03,0x12,0x03,0x19,0x10,0x11,0x0a,0x0b,0x0a,0x04, -0x04,0x03,0x02,0x01,0x12,0x03,0x1a,0x02,0x13,0x0a, -0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03, -0x1a,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, -0x01,0x01,0x12,0x03,0x1a,0x09,0x0e,0x0a,0x0c,0x0a, -0x05,0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x1a,0x11, -0x12,0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1d, -0x00,0x20,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01, -0x12,0x03,0x1d,0x08,0x33,0x0a,0x0b,0x0a,0x04,0x04, -0x04,0x02,0x00,0x12,0x03,0x1e,0x02,0x12,0x0a,0x0c, -0x0a,0x05,0x04,0x04,0x02,0x00,0x05,0x12,0x03,0x1e, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00, -0x01,0x12,0x03,0x1e,0x09,0x0d,0x0a,0x0c,0x0a,0x05, -0x04,0x04,0x02,0x00,0x03,0x12,0x03,0x1e,0x10,0x11, -0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03, -0x1f,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, -0x01,0x05,0x12,0x03,0x1f,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x1f,0x09, -0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03, -0x12,0x03,0x1f,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04, -0x05,0x12,0x04,0x22,0x00,0x25,0x01,0x0a,0x0a,0x0a, -0x03,0x04,0x05,0x01,0x12,0x03,0x22,0x08,0x2f,0x0a, -0x0b,0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x23, -0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00, -0x05,0x12,0x03,0x23,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x05,0x02,0x00,0x01,0x12,0x03,0x23,0x09,0x0d, -0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12, -0x03,0x23,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x05, -0x02,0x01,0x12,0x03,0x24,0x02,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x24,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01, -0x12,0x03,0x24,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, -0x05,0x02,0x01,0x03,0x12,0x03,0x24,0x11,0x12,0x0a, -0x0a,0x0a,0x02,0x04,0x06,0x12,0x04,0x27,0x00,0x2c, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03, -0x27,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02, -0x00,0x12,0x03,0x28,0x02,0x27,0x0a,0x0c,0x0a,0x05, -0x04,0x06,0x02,0x00,0x06,0x12,0x03,0x28,0x02,0x17, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12, -0x03,0x28,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x00,0x03,0x12,0x03,0x28,0x25,0x26,0x0a,0x0b, -0x0a,0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x29,0x02, -0x28,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x06, -0x12,0x03,0x29,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x01,0x01,0x12,0x03,0x29,0x18,0x23,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03, -0x29,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02, -0x02,0x12,0x03,0x2a,0x02,0x26,0x0a,0x0c,0x0a,0x05, -0x04,0x06,0x02,0x02,0x06,0x12,0x03,0x2a,0x02,0x17, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12, -0x03,0x2a,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x02,0x03,0x12,0x03,0x2a,0x24,0x25,0x0a,0x0b, -0x0a,0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2b,0x02, -0x27,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x06, -0x12,0x03,0x2b,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x03,0x01,0x12,0x03,0x2b,0x18,0x22,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03, -0x2b,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12, -0x04,0x2e,0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04, -0x07,0x01,0x12,0x03,0x2e,0x08,0x2a,0x0a,0x0b,0x0a, -0x04,0x04,0x07,0x02,0x00,0x12,0x03,0x2f,0x02,0x18, -0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x05,0x12, -0x03,0x2f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07, -0x02,0x00,0x01,0x12,0x03,0x2f,0x09,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x07,0x02,0x00,0x03,0x12,0x03,0x2f, -0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x01, -0x12,0x03,0x30,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04, -0x07,0x02,0x01,0x05,0x12,0x03,0x30,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x01,0x12,0x03, -0x30,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, -0x01,0x03,0x12,0x03,0x30,0x17,0x18,0x0a,0x0b,0x0a, -0x04,0x04,0x07,0x02,0x02,0x12,0x03,0x31,0x02,0x17, -0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x05,0x12, -0x03,0x31,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07, -0x02,0x02,0x01,0x12,0x03,0x31,0x09,0x12,0x0a,0x0c, -0x0a,0x05,0x04,0x07,0x02,0x02,0x03,0x12,0x03,0x31, -0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x03, -0x12,0x03,0x32,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, -0x07,0x02,0x03,0x05,0x12,0x03,0x32,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x01,0x12,0x03, -0x32,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, -0x03,0x03,0x12,0x03,0x32,0x16,0x17,0x0a,0x0a,0x0a, -0x02,0x04,0x08,0x12,0x04,0x35,0x00,0x3a,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x08,0x01,0x12,0x03,0x35,0x08, -0x27,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x00,0x12, -0x03,0x36,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08, -0x02,0x00,0x05,0x12,0x03,0x36,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x08,0x02,0x00,0x01,0x12,0x03,0x36, -0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00, -0x03,0x12,0x03,0x36,0x16,0x17,0x0a,0x0b,0x0a,0x04, -0x04,0x08,0x02,0x01,0x12,0x03,0x37,0x02,0x19,0x0a, -0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x05,0x12,0x03, -0x37,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, -0x01,0x01,0x12,0x03,0x37,0x09,0x14,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x01,0x03,0x12,0x03,0x37,0x17, -0x18,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x02,0x12, -0x03,0x38,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x08, -0x02,0x02,0x05,0x12,0x03,0x38,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x08,0x02,0x02,0x01,0x12,0x03,0x38, -0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02, -0x03,0x12,0x03,0x38,0x15,0x16,0x0a,0x0b,0x0a,0x04, -0x04,0x08,0x02,0x03,0x12,0x03,0x39,0x02,0x18,0x0a, -0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x05,0x12,0x03, -0x39,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, -0x03,0x01,0x12,0x03,0x39,0x09,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x03,0x03,0x12,0x03,0x39,0x16, -0x17,0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,0x3c, -0x00,0x41,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01, -0x12,0x03,0x3c,0x08,0x2e,0x0a,0x0b,0x0a,0x04,0x04, -0x09,0x02,0x00,0x12,0x03,0x3d,0x02,0x18,0x0a,0x0c, -0x0a,0x05,0x04,0x09,0x02,0x00,0x05,0x12,0x03,0x3d, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00, -0x01,0x12,0x03,0x3d,0x09,0x13,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x00,0x03,0x12,0x03,0x3d,0x16,0x17, -0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x01,0x12,0x03, -0x3e,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, -0x01,0x05,0x12,0x03,0x3e,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x09,0x02,0x01,0x01,0x12,0x03,0x3e,0x09, -0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x03, -0x12,0x03,0x3e,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04, -0x09,0x02,0x02,0x12,0x03,0x3f,0x02,0x17,0x0a,0x0c, -0x0a,0x05,0x04,0x09,0x02,0x02,0x05,0x12,0x03,0x3f, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x02, -0x01,0x12,0x03,0x3f,0x09,0x12,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x02,0x03,0x12,0x03,0x3f,0x15,0x16, -0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x03,0x12,0x03, -0x40,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, -0x03,0x05,0x12,0x03,0x40,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x09,0x02,0x03,0x01,0x12,0x03,0x40,0x09, -0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x03, -0x12,0x03,0x40,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04, -0x0a,0x12,0x04,0x43,0x00,0x45,0x01,0x0a,0x0a,0x0a, -0x03,0x04,0x0a,0x01,0x12,0x03,0x43,0x08,0x25,0x0a, -0x0b,0x0a,0x04,0x04,0x0a,0x02,0x00,0x12,0x03,0x44, -0x02,0x2d,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00, -0x04,0x12,0x03,0x44,0x02,0x0a,0x0a,0x0c,0x0a,0x05, -0x04,0x0a,0x02,0x00,0x06,0x12,0x03,0x44,0x0b,0x20, -0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x01,0x12, -0x03,0x44,0x21,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x0a, -0x02,0x00,0x03,0x12,0x03,0x44,0x2b,0x2c,0x0a,0x0a, -0x0a,0x02,0x04,0x0b,0x12,0x04,0x47,0x00,0x4a,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x0b,0x01,0x12,0x03,0x47, -0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02,0x00, -0x12,0x03,0x48,0x02,0x16,0x0a,0x0c,0x0a,0x05,0x04, -0x0b,0x02,0x00,0x05,0x12,0x03,0x48,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x01,0x12,0x03, -0x48,0x09,0x11,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02, -0x00,0x03,0x12,0x03,0x48,0x14,0x15,0x0a,0x0b,0x0a, -0x04,0x04,0x0b,0x02,0x01,0x12,0x03,0x49,0x02,0x1f, -0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x06,0x12, -0x03,0x49,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0b, -0x02,0x01,0x01,0x12,0x03,0x49,0x15,0x1a,0x0a,0x0c, -0x0a,0x05,0x04,0x0b,0x02,0x01,0x03,0x12,0x03,0x49, -0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12,0x04, -0x4c,0x00,0x4f,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0c, -0x01,0x12,0x03,0x4c,0x08,0x21,0x0a,0x0b,0x0a,0x04, -0x04,0x0c,0x02,0x00,0x12,0x03,0x4d,0x02,0x13,0x0a, -0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x05,0x12,0x03, -0x4d,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02, -0x00,0x01,0x12,0x03,0x4d,0x09,0x0e,0x0a,0x0c,0x0a, -0x05,0x04,0x0c,0x02,0x00,0x03,0x12,0x03,0x4d,0x11, -0x12,0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01,0x12, -0x03,0x4e,0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0c, -0x02,0x01,0x06,0x12,0x03,0x4e,0x02,0x14,0x0a,0x0c, -0x0a,0x05,0x04,0x0c,0x02,0x01,0x01,0x12,0x03,0x4e, -0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01, -0x03,0x12,0x03,0x4e,0x1d,0x1e,0x0a,0x0a,0x0a,0x02, -0x04,0x0d,0x12,0x04,0x51,0x00,0x54,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x0d,0x01,0x12,0x03,0x51,0x08,0x29, -0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02,0x00,0x12,0x03, -0x52,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02, -0x00,0x05,0x12,0x03,0x52,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x0d,0x02,0x00,0x01,0x12,0x03,0x52,0x09, -0x15,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x03, -0x12,0x03,0x52,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04, -0x0d,0x02,0x01,0x12,0x03,0x53,0x02,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x0d,0x02,0x01,0x05,0x12,0x03,0x53, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01, -0x01,0x12,0x03,0x53,0x09,0x0e,0x0a,0x0c,0x0a,0x05, -0x04,0x0d,0x02,0x01,0x03,0x12,0x03,0x53,0x11,0x12, -0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, +0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70, +0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f, +0x62,0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f, +0x6e,0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65, +0x42,0x1a,0x0a,0x18,0x65,0x64,0x75,0x2e,0x77,0x70, +0x69,0x2e,0x66,0x69,0x72,0x73,0x74,0x2e,0x6d,0x61, +0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0xd6, +0x12,0x0a,0x06,0x12,0x04,0x00,0x00,0x54,0x01,0x0a, +0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a, +0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a, +0x09,0x0a,0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x1a, +0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x31, +0x0a,0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00, +0x31,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x08, +0x00,0x0c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01, +0x12,0x03,0x08,0x08,0x1d,0x0a,0x0b,0x0a,0x04,0x04, +0x00,0x02,0x00,0x12,0x03,0x09,0x02,0x10,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x09, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00, +0x01,0x12,0x03,0x09,0x09,0x0b,0x0a,0x0c,0x0a,0x05, +0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x09,0x0e,0x0f, +0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03, +0x0a,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, +0x01,0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0a,0x09, +0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03, +0x12,0x03,0x0a,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04, +0x00,0x02,0x02,0x12,0x03,0x0b,0x02,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x0b, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02, +0x01,0x12,0x03,0x0b,0x09,0x0e,0x0a,0x0c,0x0a,0x05, +0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0b,0x11,0x12, +0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12,0x04,0x0e,0x00, +0x12,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12, +0x03,0x0e,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x01, +0x02,0x00,0x12,0x03,0x0f,0x02,0x10,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x00,0x05,0x12,0x03,0x0f,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x01, +0x12,0x03,0x0f,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04, +0x01,0x02,0x00,0x03,0x12,0x03,0x0f,0x0e,0x0f,0x0a, +0x0b,0x0a,0x04,0x04,0x01,0x02,0x01,0x12,0x03,0x10, +0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01, +0x05,0x12,0x03,0x10,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x01,0x02,0x01,0x01,0x12,0x03,0x10,0x09,0x0b, +0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x03,0x12, +0x03,0x10,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x01, +0x02,0x02,0x12,0x03,0x11,0x02,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x02,0x05,0x12,0x03,0x11,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x01, +0x12,0x03,0x11,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, +0x01,0x02,0x02,0x03,0x12,0x03,0x11,0x11,0x12,0x0a, +0x0a,0x0a,0x02,0x04,0x02,0x12,0x04,0x14,0x00,0x16, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01,0x12,0x03, +0x14,0x08,0x2b,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02, +0x00,0x12,0x03,0x15,0x02,0x18,0x0a,0x0c,0x0a,0x05, +0x04,0x02,0x02,0x00,0x05,0x12,0x03,0x15,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x01,0x12, +0x03,0x15,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02, +0x02,0x00,0x03,0x12,0x03,0x15,0x16,0x17,0x0a,0x0a, +0x0a,0x02,0x04,0x03,0x12,0x04,0x18,0x00,0x1b,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x18, +0x08,0x2c,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00, +0x12,0x03,0x19,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04, +0x03,0x02,0x00,0x05,0x12,0x03,0x19,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03, +0x19,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, +0x00,0x03,0x12,0x03,0x19,0x10,0x11,0x0a,0x0b,0x0a, +0x04,0x04,0x03,0x02,0x01,0x12,0x03,0x1a,0x02,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12, +0x03,0x1a,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03, +0x02,0x01,0x01,0x12,0x03,0x1a,0x09,0x0e,0x0a,0x0c, +0x0a,0x05,0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x1a, +0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04, +0x1d,0x00,0x20,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04, +0x01,0x12,0x03,0x1d,0x08,0x33,0x0a,0x0b,0x0a,0x04, +0x04,0x04,0x02,0x00,0x12,0x03,0x1e,0x02,0x12,0x0a, +0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x05,0x12,0x03, +0x1e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, +0x00,0x01,0x12,0x03,0x1e,0x09,0x0d,0x0a,0x0c,0x0a, +0x05,0x04,0x04,0x02,0x00,0x03,0x12,0x03,0x1e,0x10, +0x11,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12, +0x03,0x1f,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x04, +0x02,0x01,0x05,0x12,0x03,0x1f,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x1f, +0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01, +0x03,0x12,0x03,0x1f,0x11,0x12,0x0a,0x0a,0x0a,0x02, +0x04,0x05,0x12,0x04,0x22,0x00,0x25,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x05,0x01,0x12,0x03,0x22,0x08,0x2f, +0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03, +0x23,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, +0x00,0x05,0x12,0x03,0x23,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x05,0x02,0x00,0x01,0x12,0x03,0x23,0x09, +0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03, +0x12,0x03,0x23,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04, +0x05,0x02,0x01,0x12,0x03,0x24,0x02,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x24, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01, +0x01,0x12,0x03,0x24,0x09,0x0e,0x0a,0x0c,0x0a,0x05, +0x04,0x05,0x02,0x01,0x03,0x12,0x03,0x24,0x11,0x12, +0x0a,0x0a,0x0a,0x02,0x04,0x06,0x12,0x04,0x27,0x00, +0x2c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12, +0x03,0x27,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x06, +0x02,0x00,0x12,0x03,0x28,0x02,0x27,0x0a,0x0c,0x0a, +0x05,0x04,0x06,0x02,0x00,0x06,0x12,0x03,0x28,0x02, +0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01, +0x12,0x03,0x28,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04, +0x06,0x02,0x00,0x03,0x12,0x03,0x28,0x25,0x26,0x0a, +0x0b,0x0a,0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x29, +0x02,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01, +0x06,0x12,0x03,0x29,0x02,0x17,0x0a,0x0c,0x0a,0x05, +0x04,0x06,0x02,0x01,0x01,0x12,0x03,0x29,0x18,0x23, +0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12, +0x03,0x29,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06, +0x02,0x02,0x12,0x03,0x2a,0x02,0x26,0x0a,0x0c,0x0a, +0x05,0x04,0x06,0x02,0x02,0x06,0x12,0x03,0x2a,0x02, +0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01, +0x12,0x03,0x2a,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04, +0x06,0x02,0x02,0x03,0x12,0x03,0x2a,0x24,0x25,0x0a, +0x0b,0x0a,0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2b, +0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03, +0x06,0x12,0x03,0x2b,0x02,0x17,0x0a,0x0c,0x0a,0x05, +0x04,0x06,0x02,0x03,0x01,0x12,0x03,0x2b,0x18,0x22, +0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12, +0x03,0x2b,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x07, +0x12,0x04,0x2e,0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03, +0x04,0x07,0x01,0x12,0x03,0x2e,0x08,0x2a,0x0a,0x0b, +0x0a,0x04,0x04,0x07,0x02,0x00,0x12,0x03,0x2f,0x02, +0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x05, +0x12,0x03,0x2f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x07,0x02,0x00,0x01,0x12,0x03,0x2f,0x09,0x13,0x0a, +0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x03,0x12,0x03, +0x2f,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02, +0x01,0x12,0x03,0x30,0x02,0x19,0x0a,0x0c,0x0a,0x05, +0x04,0x07,0x02,0x01,0x05,0x12,0x03,0x30,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x01,0x12, +0x03,0x30,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07, +0x02,0x01,0x03,0x12,0x03,0x30,0x17,0x18,0x0a,0x0b, +0x0a,0x04,0x04,0x07,0x02,0x02,0x12,0x03,0x31,0x02, +0x17,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x05, +0x12,0x03,0x31,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x07,0x02,0x02,0x01,0x12,0x03,0x31,0x09,0x12,0x0a, +0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x03,0x12,0x03, +0x31,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02, +0x03,0x12,0x03,0x32,0x02,0x18,0x0a,0x0c,0x0a,0x05, +0x04,0x07,0x02,0x03,0x05,0x12,0x03,0x32,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x01,0x12, +0x03,0x32,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07, +0x02,0x03,0x03,0x12,0x03,0x32,0x16,0x17,0x0a,0x0a, +0x0a,0x02,0x04,0x08,0x12,0x04,0x35,0x00,0x3a,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x08,0x01,0x12,0x03,0x35, +0x08,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x00, +0x12,0x03,0x36,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x00,0x05,0x12,0x03,0x36,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x01,0x12,0x03, +0x36,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, +0x00,0x03,0x12,0x03,0x36,0x16,0x17,0x0a,0x0b,0x0a, +0x04,0x04,0x08,0x02,0x01,0x12,0x03,0x37,0x02,0x19, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x05,0x12, +0x03,0x37,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08, +0x02,0x01,0x01,0x12,0x03,0x37,0x09,0x14,0x0a,0x0c, +0x0a,0x05,0x04,0x08,0x02,0x01,0x03,0x12,0x03,0x37, +0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x02, +0x12,0x03,0x38,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x02,0x05,0x12,0x03,0x38,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x01,0x12,0x03, +0x38,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, +0x02,0x03,0x12,0x03,0x38,0x15,0x16,0x0a,0x0b,0x0a, +0x04,0x04,0x08,0x02,0x03,0x12,0x03,0x39,0x02,0x18, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x05,0x12, +0x03,0x39,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08, +0x02,0x03,0x01,0x12,0x03,0x39,0x09,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x08,0x02,0x03,0x03,0x12,0x03,0x39, +0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12,0x04, +0x3c,0x00,0x41,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09, +0x01,0x12,0x03,0x3c,0x08,0x2e,0x0a,0x0b,0x0a,0x04, +0x04,0x09,0x02,0x00,0x12,0x03,0x3d,0x02,0x18,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x05,0x12,0x03, +0x3d,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, +0x00,0x01,0x12,0x03,0x3d,0x09,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x09,0x02,0x00,0x03,0x12,0x03,0x3d,0x16, +0x17,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x01,0x12, +0x03,0x3e,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x01,0x05,0x12,0x03,0x3e,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x01,0x01,0x12,0x03,0x3e, +0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01, +0x03,0x12,0x03,0x3e,0x17,0x18,0x0a,0x0b,0x0a,0x04, +0x04,0x09,0x02,0x02,0x12,0x03,0x3f,0x02,0x17,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x05,0x12,0x03, +0x3f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, +0x02,0x01,0x12,0x03,0x3f,0x09,0x12,0x0a,0x0c,0x0a, +0x05,0x04,0x09,0x02,0x02,0x03,0x12,0x03,0x3f,0x15, +0x16,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x03,0x12, +0x03,0x40,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x03,0x05,0x12,0x03,0x40,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x03,0x01,0x12,0x03,0x40, +0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03, +0x03,0x12,0x03,0x40,0x16,0x17,0x0a,0x0a,0x0a,0x02, +0x04,0x0a,0x12,0x04,0x43,0x00,0x45,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x0a,0x01,0x12,0x03,0x43,0x08,0x25, +0x0a,0x0b,0x0a,0x04,0x04,0x0a,0x02,0x00,0x12,0x03, +0x44,0x02,0x2d,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02, +0x00,0x04,0x12,0x03,0x44,0x02,0x0a,0x0a,0x0c,0x0a, +0x05,0x04,0x0a,0x02,0x00,0x06,0x12,0x03,0x44,0x0b, +0x20,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x01, +0x12,0x03,0x44,0x21,0x28,0x0a,0x0c,0x0a,0x05,0x04, +0x0a,0x02,0x00,0x03,0x12,0x03,0x44,0x2b,0x2c,0x0a, +0x0a,0x0a,0x02,0x04,0x0b,0x12,0x04,0x47,0x00,0x4a, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0b,0x01,0x12,0x03, +0x47,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02, +0x00,0x12,0x03,0x48,0x02,0x16,0x0a,0x0c,0x0a,0x05, +0x04,0x0b,0x02,0x00,0x05,0x12,0x03,0x48,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x01,0x12, +0x03,0x48,0x09,0x11,0x0a,0x0c,0x0a,0x05,0x04,0x0b, +0x02,0x00,0x03,0x12,0x03,0x48,0x14,0x15,0x0a,0x0b, +0x0a,0x04,0x04,0x0b,0x02,0x01,0x12,0x03,0x49,0x02, +0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x06, +0x12,0x03,0x49,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04, +0x0b,0x02,0x01,0x01,0x12,0x03,0x49,0x15,0x1a,0x0a, +0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x03,0x12,0x03, +0x49,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12, +0x04,0x4c,0x00,0x4f,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x0c,0x01,0x12,0x03,0x4c,0x08,0x21,0x0a,0x0b,0x0a, +0x04,0x04,0x0c,0x02,0x00,0x12,0x03,0x4d,0x02,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x05,0x12, +0x03,0x4d,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0c, +0x02,0x00,0x01,0x12,0x03,0x4d,0x09,0x0e,0x0a,0x0c, +0x0a,0x05,0x04,0x0c,0x02,0x00,0x03,0x12,0x03,0x4d, +0x11,0x12,0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01, +0x12,0x03,0x4e,0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04, +0x0c,0x02,0x01,0x06,0x12,0x03,0x4e,0x02,0x14,0x0a, +0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x01,0x12,0x03, +0x4e,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02, +0x01,0x03,0x12,0x03,0x4e,0x1d,0x1e,0x0a,0x0a,0x0a, +0x02,0x04,0x0d,0x12,0x04,0x51,0x00,0x54,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x0d,0x01,0x12,0x03,0x51,0x08, +0x29,0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02,0x00,0x12, +0x03,0x52,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d, +0x02,0x00,0x05,0x12,0x03,0x52,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x0d,0x02,0x00,0x01,0x12,0x03,0x52, +0x09,0x15,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00, +0x03,0x12,0x03,0x52,0x18,0x19,0x0a,0x0b,0x0a,0x04, +0x04,0x0d,0x02,0x01,0x12,0x03,0x53,0x02,0x1f,0x0a, +0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x06,0x12,0x03, +0x53,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02, +0x01,0x01,0x12,0x03,0x53,0x15,0x1a,0x0a,0x0c,0x0a, +0x05,0x04,0x0d,0x02,0x01,0x03,0x12,0x03,0x53,0x1d, +0x1e,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, }; static const char file_name[] = "kinematics.proto"; static const char wpi_proto_ProtobufChassisSpeeds_name[] = "wpi.proto.ProtobufChassisSpeeds"; diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h index 97fff24e554..80b176bea1d 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h @@ -147,7 +147,7 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { static pb_filedesc_t file_descriptor(void) noexcept; double acceleration; - double angle; + pb_callback_t angle; } wpi_proto_ProtobufSwerveModuleAccelerations; @@ -165,7 +165,7 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { #define wpi_proto_ProtobufSwerveDriveKinematics_init_default {{{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModulePosition_init_default {0, {{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModuleState_init_default {0, {{NULL}, NULL}} -#define wpi_proto_ProtobufSwerveModuleAccelerations_init_default {0, 0} +#define wpi_proto_ProtobufSwerveModuleAccelerations_init_default {0, {{NULL}, NULL}} #define wpi_proto_ProtobufChassisSpeeds_init_zero {0, 0, 0} #define wpi_proto_ProtobufChassisAccelerations_init_zero {0, 0, 0} #define wpi_proto_ProtobufDifferentialDriveKinematics_init_zero {0} @@ -179,7 +179,7 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { #define wpi_proto_ProtobufSwerveDriveKinematics_init_zero {{{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModulePosition_init_zero {0, {{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModuleState_init_zero {0, {{NULL}, NULL}} -#define wpi_proto_ProtobufSwerveModuleAccelerations_init_zero {0, 0} +#define wpi_proto_ProtobufSwerveModuleAccelerations_init_zero {0, {{NULL}, NULL}} /* Field tags (for use in manual encoding/decoding) */ #define wpi_proto_ProtobufChassisSpeeds_vx_tag 1 @@ -315,15 +315,17 @@ X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2) #define wpi_proto_ProtobufSwerveModuleAccelerations_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, DOUBLE, acceleration, 1) \ -X(a, STATIC, SINGULAR, DOUBLE, angle, 2) -#define wpi_proto_ProtobufSwerveModuleAccelerations_CALLBACK NULL +X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2) +#define wpi_proto_ProtobufSwerveModuleAccelerations_CALLBACK pb_default_field_callback #define wpi_proto_ProtobufSwerveModuleAccelerations_DEFAULT NULL +#define wpi_proto_ProtobufSwerveModuleAccelerations_angle_MSGTYPE wpi_proto_ProtobufRotation2d /* Maximum encoded size of messages (where known) */ /* wpi_proto_ProtobufMecanumDriveKinematics_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveDriveKinematics_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveModulePosition_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveModuleState_size depends on runtime parameters */ +/* wpi_proto_ProtobufSwerveModuleAccelerations_size depends on runtime parameters */ #define WPI_PROTO_KINEMATICS_NPB_H_MAX_SIZE wpi_proto_ProtobufMecanumDriveWheelPositions_size #define wpi_proto_ProtobufChassisAccelerations_size 27 #define wpi_proto_ProtobufChassisSpeeds_size 27 @@ -334,7 +336,6 @@ X(a, STATIC, SINGULAR, DOUBLE, angle, 2) #define wpi_proto_ProtobufMecanumDriveWheelAccelerations_size 36 #define wpi_proto_ProtobufMecanumDriveWheelPositions_size 36 #define wpi_proto_ProtobufMecanumDriveWheelSpeeds_size 36 -#define wpi_proto_ProtobufSwerveModuleAccelerations_size 18 #endif diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java index c5228892166..caf692c38fc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleAccelerationsProto.java @@ -29,12 +29,13 @@ public ProtobufSwerveModuleAccelerations createMessage() { @Override public SwerveModuleAccelerations unpack(ProtobufSwerveModuleAccelerations msg) { - return new SwerveModuleAccelerations(msg.getAcceleration(), new Rotation2d(msg.getAngle())); + return new SwerveModuleAccelerations( + msg.getAcceleration(), Rotation2d.proto.unpack(msg.getAngle())); } @Override public void pack(ProtobufSwerveModuleAccelerations msg, SwerveModuleAccelerations value) { msg.setAcceleration(value.acceleration); - msg.setAngle(value.angle.getRadians()); + Rotation2d.proto.pack(msg.getAngle(), value.angle); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java index aa4a37c7fbb..0bf011e7062 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java @@ -22,24 +22,24 @@ public String getTypeName() { @Override public int getSize() { - return kSizeDouble * 2; + return kSizeDouble + Rotation2d.struct.getSize(); } @Override public String getSchema() { - return "double acceleration;double angle"; + return "double acceleration;Rotation2d angle"; } @Override public SwerveModuleAccelerations unpack(ByteBuffer bb) { double acceleration = bb.getDouble(); - double angle = bb.getDouble(); - return new SwerveModuleAccelerations(acceleration, new Rotation2d(angle)); + Rotation2d angle = Rotation2d.struct.unpack(bb); + return new SwerveModuleAccelerations(acceleration, angle); } @Override public void pack(ByteBuffer bb, SwerveModuleAccelerations value) { bb.putDouble(value.acceleration); - bb.putDouble(value.angle.getRadians()); + Rotation2d.struct.pack(bb, value.angle); } } diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp index a433096a33e..69761a18440 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp @@ -4,26 +4,39 @@ #include "frc/kinematics/proto/SwerveModuleAccelerationsProto.h" +#include + #include "wpimath/protobuf/kinematics.npb.h" -std::optional wpi::Protobuf::Unpack( - InputStream& stream) { - wpi_proto_ProtobufSwerveModuleAccelerations msg; +std::optional +wpi::Protobuf::Unpack(InputStream& stream) { + wpi::UnpackCallback angle; + wpi_proto_ProtobufSwerveModuleAccelerations msg{ + .acceleration = 0, + .angle = angle.Callback(), +}; if (!stream.Decode(msg)) { return {}; } + auto iangle = angle.Items(); + + if (iangle.empty()) { + return {}; + } + return frc::SwerveModuleAccelerations{ - units::meters_per_second_squared_t{msg.acceleration}, - frc::Rotation2d{units::radian_t{msg.angle}}, - }; + units::meters_per_second_squared_t{msg.acceleration}, + iangle[0], +}; } -bool wpi::Protobuf::Pack(OutputStream& stream, - const frc::SwerveModuleAccelerations& value) { +bool wpi::Protobuf::Pack( + OutputStream& stream, const frc::SwerveModuleAccelerations& value) { + wpi::PackCallback angle{&value.angle}; wpi_proto_ProtobufSwerveModuleAccelerations msg{ - .acceleration = value.acceleration.value(), - .angle = value.angle.Radians().value(), - }; + .acceleration = value.acceleration.value(), + .angle = angle.Callback(), +}; return stream.Encode(msg); } diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp index 797237de17d..ceb12e271b6 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp @@ -15,8 +15,7 @@ frc::SwerveModuleAccelerations wpi::Struct::Unpa return frc::SwerveModuleAccelerations{ units::meters_per_second_squared_t{ wpi::UnpackStruct(data)}, - frc::Rotation2d{units::radian_t{ - wpi::UnpackStruct(data)}}, + wpi::UnpackStruct(data) }; } @@ -25,5 +24,5 @@ void wpi::Struct::Pack( constexpr size_t kAccelerationOff = 0; constexpr size_t kAngleOff = kAccelerationOff + 8; wpi::PackStruct(data, value.acceleration.value()); - wpi::PackStruct(data, value.angle.Radians().value()); + wpi::PackStruct(data, value.angle); } diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h index 59c389b5478..2d469f8d65a 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h @@ -12,9 +12,9 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { static constexpr std::string_view GetTypeName() { return "SwerveModuleAccelerations"; } - static constexpr size_t GetSize() { return 16; } + static constexpr size_t GetSize() { return 8 + wpi::GetStructSize(); } static constexpr std::string_view GetSchema() { - return "double acceleration;double angle"; + return "double acceleration;Rotation2d angle"; } static frc::SwerveModuleAccelerations Unpack(std::span data); diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto index 82b95ff585e..6c27c5c2560 100644 --- a/wpimath/src/main/proto/kinematics.proto +++ b/wpimath/src/main/proto/kinematics.proto @@ -81,5 +81,5 @@ message ProtobufSwerveModuleState { message ProtobufSwerveModuleAccelerations { double acceleration = 1; - double angle = 2; + ProtobufRotation2d angle = 2; } From ae34fa1ec20a240d259789abc608221cc29afc4c Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Mon, 25 Aug 2025 15:16:29 -0400 Subject: [PATCH 25/82] refactor: run wpiformat Signed-off-by: Zach Harel --- .../proto/ChassisAccelerationsProto.cpp | 8 +++--- ...fferentialDriveWheelAccelerationsProto.cpp | 9 +++--- .../MecanumDriveWheelAccelerationsProto.cpp | 8 +++--- .../proto/SwerveModuleAccelerationsProto.cpp | 18 ++++++------ ...ferentialDriveWheelAccelerationsStruct.cpp | 6 ++-- .../MecanumDriveWheelAccelerationsStruct.cpp | 3 +- .../SwerveModuleAccelerationsStruct.cpp | 7 ++--- .../frc/kinematics/ChassisAccelerations.h | 28 +++++++++++-------- .../DifferentialDriveWheelAccelerations.h | 11 ++++---- .../MecanumDriveWheelAccelerations.h | 7 +++-- .../kinematics/SwerveModuleAccelerations.h | 6 ++-- .../proto/ChassisAccelerationsProto.h | 3 +- ...DifferentialDriveWheelAccelerationsProto.h | 15 ++++++---- .../MecanumDriveWheelAccelerationsProto.h | 12 +++++--- .../proto/SwerveModuleAccelerationsProto.h | 6 ++-- .../struct/ChassisAccelerationsStruct.h | 7 +++-- ...ifferentialDriveWheelAccelerationsStruct.h | 13 ++++++--- .../MecanumDriveWheelAccelerationsStruct.h | 13 ++++++--- .../struct/SwerveModuleAccelerationsStruct.h | 11 ++++++-- 19 files changed, 117 insertions(+), 74 deletions(-) diff --git a/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp index d2f9cc1ba57..425e78730ba 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp @@ -6,8 +6,8 @@ #include "wpimath/protobuf/kinematics.npb.h" -std::optional wpi::Protobuf::Unpack( - InputStream& stream) { +std::optional +wpi::Protobuf::Unpack(InputStream& stream) { wpi_proto_ProtobufChassisAccelerations msg; if (!stream.Decode(msg)) { return {}; @@ -20,8 +20,8 @@ std::optional wpi::Protobuf::Pack(OutputStream& stream, - const frc::ChassisAccelerations& value) { +bool wpi::Protobuf::Pack( + OutputStream& stream, const frc::ChassisAccelerations& value) { wpi_proto_ProtobufChassisAccelerations msg{ .ax = value.ax.value(), .ay = value.ay.value(), diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp index 72e61d9b610..0148dfabd56 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp @@ -6,8 +6,8 @@ #include "wpimath/protobuf/kinematics.npb.h" -std::optional wpi::Protobuf::Unpack( - InputStream& stream) { +std::optional wpi::Protobuf< + frc::DifferentialDriveWheelAccelerations>::Unpack(InputStream& stream) { wpi_proto_ProtobufDifferentialDriveWheelAccelerations msg; if (!stream.Decode(msg)) { return {}; @@ -19,8 +19,9 @@ std::optional wpi::Protobuf::Pack(OutputStream& stream, - const frc::DifferentialDriveWheelAccelerations& value) { +bool wpi::Protobuf::Pack( + OutputStream& stream, + const frc::DifferentialDriveWheelAccelerations& value) { wpi_proto_ProtobufDifferentialDriveWheelAccelerations msg{ .left = value.left.value(), .right = value.right.value(), diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp index 77a5149aa3b..e024ee97e5c 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp @@ -6,8 +6,8 @@ #include "wpimath/protobuf/kinematics.npb.h" -std::optional wpi::Protobuf::Unpack( - InputStream& stream) { +std::optional wpi::Protobuf< + frc::MecanumDriveWheelAccelerations>::Unpack(InputStream& stream) { wpi_proto_ProtobufMecanumDriveWheelAccelerations msg; if (!stream.Decode(msg)) { return {}; @@ -21,8 +21,8 @@ std::optional wpi::Protobuf::Pack(OutputStream& stream, - const frc::MecanumDriveWheelAccelerations& value) { +bool wpi::Protobuf::Pack( + OutputStream& stream, const frc::MecanumDriveWheelAccelerations& value) { wpi_proto_ProtobufMecanumDriveWheelAccelerations msg{ .front_left = value.frontLeft.value(), .front_right = value.frontRight.value(), diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp index 69761a18440..6b9a5c73f3c 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp @@ -12,9 +12,9 @@ std::optional wpi::Protobuf::Unpack(InputStream& stream) { wpi::UnpackCallback angle; wpi_proto_ProtobufSwerveModuleAccelerations msg{ - .acceleration = 0, - .angle = angle.Callback(), -}; + .acceleration = 0, + .angle = angle.Callback(), + }; if (!stream.Decode(msg)) { return {}; } @@ -26,17 +26,17 @@ wpi::Protobuf::Unpack(InputStream& stream) { } return frc::SwerveModuleAccelerations{ - units::meters_per_second_squared_t{msg.acceleration}, - iangle[0], -}; + units::meters_per_second_squared_t{msg.acceleration}, + iangle[0], + }; } bool wpi::Protobuf::Pack( OutputStream& stream, const frc::SwerveModuleAccelerations& value) { wpi::PackCallback angle{&value.angle}; wpi_proto_ProtobufSwerveModuleAccelerations msg{ - .acceleration = value.acceleration.value(), - .angle = angle.Callback(), -}; + .acceleration = value.acceleration.value(), + .angle = angle.Callback(), + }; return stream.Encode(msg); } diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp index a21ea36a1d2..40867e5a4db 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp @@ -8,7 +8,8 @@ #include "frc/kinematics/DifferentialDriveWheelAccelerations.h" -frc::DifferentialDriveWheelAccelerations wpi::Struct::Unpack( +frc::DifferentialDriveWheelAccelerations +wpi::Struct::Unpack( std::span data) { constexpr size_t kLeftOff = 0; constexpr size_t kRightOff = kLeftOff + 8; @@ -21,7 +22,8 @@ frc::DifferentialDriveWheelAccelerations wpi::Struct::Pack( - std::span data, const frc::DifferentialDriveWheelAccelerations& value) { + std::span data, + const frc::DifferentialDriveWheelAccelerations& value) { constexpr size_t kLeftOff = 0; constexpr size_t kRightOff = kLeftOff + 8; wpi::PackStruct(data, value.left.value()); diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp index 164eea67919..6ef8e5c4eb6 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp @@ -8,7 +8,8 @@ #include "frc/kinematics/MecanumDriveWheelAccelerations.h" -frc::MecanumDriveWheelAccelerations wpi::Struct::Unpack( +frc::MecanumDriveWheelAccelerations +wpi::Struct::Unpack( std::span data) { constexpr size_t kFrontLeftOff = 0; constexpr size_t kFrontRightOff = kFrontLeftOff + 8; diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp index ceb12e271b6..e3217bfc459 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp @@ -8,15 +8,14 @@ #include "frc/kinematics/SwerveModuleAccelerations.h" -frc::SwerveModuleAccelerations wpi::Struct::Unpack( - std::span data) { +frc::SwerveModuleAccelerations wpi::Struct< + frc::SwerveModuleAccelerations>::Unpack(std::span data) { constexpr size_t kAccelerationOff = 0; constexpr size_t kAngleOff = kAccelerationOff + 8; return frc::SwerveModuleAccelerations{ units::meters_per_second_squared_t{ wpi::UnpackStruct(data)}, - wpi::UnpackStruct(data) - }; + wpi::UnpackStruct(data)}; } void wpi::Struct::Pack( diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h index 1bceaeebc8d..c2bbfd2547b 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h @@ -84,29 +84,31 @@ struct WPILIB_DLLEXPORT ChassisAccelerations { /** * Adds two ChassisAccelerations and returns the sum. * - *

For example, ChassisAccelerations{1.0, 0.5, 1.5} + ChassisAccelerations{2.0, 1.5, 0.5} - * = ChassisAccelerations{3.0, 2.0, 2.0} + *

For example, ChassisAccelerations{1.0, 0.5, 1.5} + + * ChassisAccelerations{2.0, 1.5, 0.5} = ChassisAccelerations{3.0, 2.0, 2.0} * * @param other The ChassisAccelerations to add. * * @return The sum of the ChassisAccelerations. */ - constexpr ChassisAccelerations operator+(const ChassisAccelerations& other) const { + constexpr ChassisAccelerations operator+( + const ChassisAccelerations& other) const { return {ax + other.ax, ay + other.ay, alpha + other.alpha}; } /** - * Subtracts the other ChassisAccelerations from the current ChassisAccelerations and - * returns the difference. + * Subtracts the other ChassisAccelerations from the current + * ChassisAccelerations and returns the difference. * - *

For example, ChassisAccelerations{5.0, 4.0, 2.0} - ChassisAccelerations{1.0, 2.0, 1.0} - * = ChassisAccelerations{4.0, 2.0, 1.0} + *

For example, ChassisAccelerations{5.0, 4.0, 2.0} - + * ChassisAccelerations{1.0, 2.0, 1.0} = ChassisAccelerations{4.0, 2.0, 1.0} * * @param other The ChassisAccelerations to subtract. * * @return The difference between the two ChassisAccelerations. */ - constexpr ChassisAccelerations operator-(const ChassisAccelerations& other) const { + constexpr ChassisAccelerations operator-( + const ChassisAccelerations& other) const { return *this + -other; } @@ -116,10 +118,13 @@ struct WPILIB_DLLEXPORT ChassisAccelerations { * * @return The inverse of the current ChassisAccelerations. */ - constexpr ChassisAccelerations operator-() const { return {-ax, -ay, -alpha}; } + constexpr ChassisAccelerations operator-() const { + return {-ax, -ay, -alpha}; + } /** - * Multiplies the ChassisAccelerations by a scalar and returns the new ChassisAccelerations. + * Multiplies the ChassisAccelerations by a scalar and returns the new + * ChassisAccelerations. * *

For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2 * = ChassisAccelerations{4.0, 5.0, 1.0} @@ -133,7 +138,8 @@ struct WPILIB_DLLEXPORT ChassisAccelerations { } /** - * Divides the ChassisAccelerations by a scalar and returns the new ChassisAccelerations. + * Divides the ChassisAccelerations by a scalar and returns the new + * ChassisAccelerations. * *

For example, ChassisAccelerations{2.0, 2.5, 1.0} / 2 * = ChassisAccelerations{1.0, 1.25, 0.5} diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelAccelerations.h index 2446bfb9557..b48617a460b 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelAccelerations.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelAccelerations.h @@ -69,8 +69,8 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations { } /** - * Multiplies the DifferentialDriveWheelAccelerations by a scalar and returns the new - * DifferentialDriveWheelAccelerations. + * Multiplies the DifferentialDriveWheelAccelerations by a scalar and returns + * the new DifferentialDriveWheelAccelerations. * *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2 * = DifferentialDriveWheelAccelerations{4.0, 5.0} @@ -84,8 +84,8 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations { } /** - * Divides the DifferentialDriveWheelAccelerations by a scalar and returns the new - * DifferentialDriveWheelAccelerations. + * Divides the DifferentialDriveWheelAccelerations by a scalar and returns the + * new DifferentialDriveWheelAccelerations. * *

For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2 * = DifferentialDriveWheelAccelerations{1.0, 1.25} @@ -105,7 +105,8 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations { * * @return True if the DifferentialDriveWheelAccelerations are equal. */ - constexpr bool operator==(const DifferentialDriveWheelAccelerations& other) const = default; + constexpr bool operator==( + const DifferentialDriveWheelAccelerations& other) const = default; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelAccelerations.h index c5e80ac95c5..8b2530c24cf 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelAccelerations.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelAccelerations.h @@ -82,8 +82,8 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations { } /** - * Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the new - * MecanumDriveWheelAccelerations. + * Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the + * new MecanumDriveWheelAccelerations. * *

For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 = * MecanumDriveWheelAccelerations{4.0, 5.0, 6.0, 7.0} @@ -117,7 +117,8 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations { * * @return True if the MecanumDriveWheelAccelerations are equal. */ - constexpr bool operator==(const MecanumDriveWheelAccelerations& other) const = default; + constexpr bool operator==(const MecanumDriveWheelAccelerations& other) const = + default; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h index ee8032af21c..0c6480570eb 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h @@ -43,7 +43,8 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * @param other The SwerveModuleAccelerations to add. * @return The sum of the SwerveModuleAccelerations. */ - SwerveModuleAccelerations operator+(const SwerveModuleAccelerations& other) const { + SwerveModuleAccelerations operator+( + const SwerveModuleAccelerations& other) const { // Convert to Cartesian coordinates, add, then convert back auto thisX = acceleration * angle.Cos(); auto thisY = acceleration * angle.Sin(); @@ -66,7 +67,8 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * @param other The SwerveModuleAccelerations to subtract. * @return The difference between the two SwerveModuleAccelerations. */ - SwerveModuleAccelerations operator-(const SwerveModuleAccelerations& other) const { + SwerveModuleAccelerations operator-( + const SwerveModuleAccelerations& other) const { return *this + (-other); } diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/ChassisAccelerationsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/ChassisAccelerationsProto.h index d4fc920fc14..7c92cf7ed67 100644 --- a/wpimath/src/main/native/include/frc/kinematics/proto/ChassisAccelerationsProto.h +++ b/wpimath/src/main/native/include/frc/kinematics/proto/ChassisAccelerationsProto.h @@ -16,5 +16,6 @@ struct WPILIB_DLLEXPORT wpi::Protobuf { using InputStream = wpi::ProtoInputStream; using OutputStream = wpi::ProtoOutputStream; static std::optional Unpack(InputStream& stream); - static bool Pack(OutputStream& stream, const frc::ChassisAccelerations& value); + static bool Pack(OutputStream& stream, + const frc::ChassisAccelerations& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h index bf5a7590b02..506718ad68c 100644 --- a/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h +++ b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h @@ -11,10 +11,15 @@ #include "wpimath/protobuf/kinematics.npb.h" template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { +struct WPILIB_DLLEXPORT + wpi::Protobuf { using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelAccelerations; - using InputStream = wpi::ProtoInputStream; - using OutputStream = wpi::ProtoOutputStream; - static std::optional Unpack(InputStream& stream); - static bool Pack(OutputStream& stream, const frc::DifferentialDriveWheelAccelerations& value); + using InputStream = + wpi::ProtoInputStream; + using OutputStream = + wpi::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const frc::DifferentialDriveWheelAccelerations& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h index 1b9a0206f26..88e5fdf4dca 100644 --- a/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h +++ b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h @@ -13,8 +13,12 @@ template <> struct WPILIB_DLLEXPORT wpi::Protobuf { using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelAccelerations; - using InputStream = wpi::ProtoInputStream; - using OutputStream = wpi::ProtoOutputStream; - static std::optional Unpack(InputStream& stream); - static bool Pack(OutputStream& stream, const frc::MecanumDriveWheelAccelerations& value); + using InputStream = + wpi::ProtoInputStream; + using OutputStream = + wpi::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const frc::MecanumDriveWheelAccelerations& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleAccelerationsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleAccelerationsProto.h index df02829ad46..e898e88aef8 100644 --- a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleAccelerationsProto.h +++ b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleAccelerationsProto.h @@ -15,6 +15,8 @@ struct WPILIB_DLLEXPORT wpi::Protobuf { using MessageStruct = wpi_proto_ProtobufSwerveModuleAccelerations; using InputStream = wpi::ProtoInputStream; using OutputStream = wpi::ProtoOutputStream; - static std::optional Unpack(InputStream& stream); - static bool Pack(OutputStream& stream, const frc::SwerveModuleAccelerations& value); + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const frc::SwerveModuleAccelerations& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/ChassisAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisAccelerationsStruct.h index 34b45bf0fa6..8087dd4d477 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/ChassisAccelerationsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisAccelerationsStruct.h @@ -11,14 +11,17 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view GetTypeName() { return "ChassisAccelerations"; } + static constexpr std::string_view GetTypeName() { + return "ChassisAccelerations"; + } static constexpr size_t GetSize() { return 24; } static constexpr std::string_view GetSchema() { return "double ax;double ay;double alpha"; } static frc::ChassisAccelerations Unpack(std::span data); - static void Pack(std::span data, const frc::ChassisAccelerations& value); + static void Pack(std::span data, + const frc::ChassisAccelerations& value); }; static_assert(wpi::StructSerializable); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h index ff7483f27b9..ea20bdbff53 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h @@ -11,14 +11,19 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view GetTypeName() { return "DifferentialDriveWheelAccelerations"; } + static constexpr std::string_view GetTypeName() { + return "DifferentialDriveWheelAccelerations"; + } static constexpr size_t GetSize() { return 16; } static constexpr std::string_view GetSchema() { return "double left;double right"; } - static frc::DifferentialDriveWheelAccelerations Unpack(std::span data); - static void Pack(std::span data, const frc::DifferentialDriveWheelAccelerations& value); + static frc::DifferentialDriveWheelAccelerations Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveWheelAccelerations& value); }; -static_assert(wpi::StructSerializable); +static_assert( + wpi::StructSerializable); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h index 61ae0be871e..21438e88056 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h @@ -11,14 +11,19 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view GetTypeName() { return "MecanumDriveWheelAccelerations"; } + static constexpr std::string_view GetTypeName() { + return "MecanumDriveWheelAccelerations"; + } static constexpr size_t GetSize() { return 32; } static constexpr std::string_view GetSchema() { - return "double front_left;double front_right;double rear_left;double rear_right"; + return "double front_left;double front_right;double rear_left;double " + "rear_right"; } - static frc::MecanumDriveWheelAccelerations Unpack(std::span data); - static void Pack(std::span data, const frc::MecanumDriveWheelAccelerations& value); + static frc::MecanumDriveWheelAccelerations Unpack( + std::span data); + static void Pack(std::span data, + const frc::MecanumDriveWheelAccelerations& value); }; static_assert(wpi::StructSerializable); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h index 2d469f8d65a..85ca32e936f 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h @@ -11,14 +11,19 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view GetTypeName() { return "SwerveModuleAccelerations"; } - static constexpr size_t GetSize() { return 8 + wpi::GetStructSize(); } + static constexpr std::string_view GetTypeName() { + return "SwerveModuleAccelerations"; + } + static constexpr size_t GetSize() { + return 8 + wpi::GetStructSize(); + } static constexpr std::string_view GetSchema() { return "double acceleration;Rotation2d angle"; } static frc::SwerveModuleAccelerations Unpack(std::span data); - static void Pack(std::span data, const frc::SwerveModuleAccelerations& value); + static void Pack(std::span data, + const frc::SwerveModuleAccelerations& value); }; static_assert(wpi::StructSerializable); From d773c96f825fe316b1d0deb0c05da7bcfcdde103 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 28 Aug 2025 13:33:14 -0400 Subject: [PATCH 26/82] refactor: clarify ChassisAccelerations documentation and fix example multiplication Signed-off-by: Zach Harel --- .../frc/kinematics/ChassisAccelerations.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h index c2bbfd2547b..34e7a3f07a5 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h @@ -8,20 +8,20 @@ #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Translation2d.h" +#include "frc/MathUtil.h" #include "units/acceleration.h" #include "units/angular_acceleration.h" namespace frc { /** * Represents the acceleration of a robot chassis. Although this struct contains - * similar members compared to a Twist2d, they do NOT represent the same thing. - * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of - * reference, a ChassisAccelerations struct represents a robot's acceleration. + * similar members compared to a ChassisSpeeds, they do NOT represent the same thing. + * Whereas a ChassisSpeeds object represents a robot's velocity, a ChassisAccelerations + * object represents a robot's acceleration. * * A strictly non-holonomic drivetrain, such as a differential drive, should - * never have an ay component because it can never accelerate sideways. - * Holonomic drivetrains such as swerve and mecanum will often have all three - * components. + * never have an ay component because it can never move sideways. Holonomic + * drivetrains such as swerve and mecanum will often have all three components. */ struct WPILIB_DLLEXPORT ChassisAccelerations { /** @@ -35,7 +35,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations { units::meters_per_second_squared_t ay = 0_mps_sq; /** - * Represents the angular acceleration of the robot frame. (CCW is +) + * Angular acceleration of the robot frame. (CCW is +) */ units::radians_per_second_squared_t alpha = 0_rad_per_s_sq; @@ -127,7 +127,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations { * ChassisAccelerations. * *

For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2 - * = ChassisAccelerations{4.0, 5.0, 1.0} + * = ChassisAccelerations{4.0, 5.0, 2.0} * * @param scalar The scalar to multiply by. * From 9b35978867a0efa590e6ce8e02352a457ebdc6d9 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 29 Aug 2025 10:39:20 -0400 Subject: [PATCH 27/82] refactor: reorganize includes and improve ChassisAccelerations documentation formatting Signed-off-by: Zach Harel --- .../native/include/frc/kinematics/ChassisAccelerations.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h index 34e7a3f07a5..16d434c814d 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisAccelerations.h @@ -6,18 +6,18 @@ #include +#include "frc/MathUtil.h" #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Translation2d.h" -#include "frc/MathUtil.h" #include "units/acceleration.h" #include "units/angular_acceleration.h" namespace frc { /** * Represents the acceleration of a robot chassis. Although this struct contains - * similar members compared to a ChassisSpeeds, they do NOT represent the same thing. - * Whereas a ChassisSpeeds object represents a robot's velocity, a ChassisAccelerations - * object represents a robot's acceleration. + * similar members compared to a ChassisSpeeds, they do NOT represent the same + * thing. Whereas a ChassisSpeeds object represents a robot's velocity, a + * ChassisAccelerations object represents a robot's acceleration. * * A strictly non-holonomic drivetrain, such as a differential drive, should * never have an ay component because it can never move sideways. Holonomic From 39326d3f77f58f4cbfdd6fe23daec5c68704837f Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 29 Aug 2025 20:19:57 -0400 Subject: [PATCH 28/82] refactor: reorganize includes and improve ChassisAccelerations documentation formatting Signed-off-by: Zach Harel --- .../edu/wpi/first/math/kinematics/ChassisAccelerations.java | 4 +--- .../native/include/frc/kinematics/SwerveModuleAccelerations.h | 4 ++-- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java index eef24c03b2e..a84d842c2d5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java @@ -20,9 +20,7 @@ import java.util.Objects; /** - * Represents the acceleration of a robot chassis. Although this class contains similar members - * compared to a ChassisSpeeds, they do NOT represent the same thing. Whereas a ChassisSpeeds object - * represents a robot's velocity, a ChassisAccelerations object represents a robot's acceleration. + * Represents the acceleration of a robot chassis. * *

A strictly non-holonomic drivetrain, such as a differential drive, should never have an ay * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h index 0c6480570eb..900f574bdc7 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleAccelerations.h @@ -74,12 +74,12 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { /** * Returns the inverse of the current SwerveModuleAccelerations. - * This is equivalent to negating the acceleration magnitude. + * This is equivalent to rotating the acceleration by pi (or 180 degrees). * * @return The inverse of the current SwerveModuleAccelerations. */ constexpr SwerveModuleAccelerations operator-() const { - return {-acceleration, angle + Rotation2d{180_deg}}; + return {acceleration, angle + Rotation2d{180_deg}}; } /** From d526033925f7b007d7542625237dbf3b71700aac Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 26 Sep 2025 15:49:08 -0400 Subject: [PATCH 29/82] refactor: replace MathUtil.interpolate() with MathUtil.lerp() in ChassisAccelerations and ChassisSpeeds Signed-off-by: Zach Harel --- .../edu/wpi/first/math/kinematics/ChassisAccelerations.java | 6 +++--- .../java/edu/wpi/first/math/kinematics/ChassisSpeeds.java | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java index a84d842c2d5..05cf44b331c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisAccelerations.java @@ -175,9 +175,9 @@ public ChassisAccelerations interpolate(ChassisAccelerations endValue, double t) return endValue; } else { return new ChassisAccelerations( - MathUtil.interpolate(this.ax, endValue.ax, t), - MathUtil.interpolate(this.ay, endValue.ay, t), - MathUtil.interpolate(this.alpha, endValue.alpha, t)); + MathUtil.lerp(this.ax, endValue.ax, t), + MathUtil.lerp(this.ay, endValue.ay, t), + MathUtil.lerp(this.alpha, endValue.alpha, t)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 50b5d8ad044..63bf7c5b375 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -210,9 +210,9 @@ public ChassisSpeeds interpolate(ChassisSpeeds endValue, double t) { return endValue; } else { return new ChassisSpeeds( - MathUtil.interpolate(this.vx, endValue.vx, t), - MathUtil.interpolate(this.vy, endValue.vy, t), - MathUtil.interpolate(this.omega, endValue.omega, t)); + MathUtil.lerp(this.vx, endValue.vx, t), + MathUtil.lerp(this.vy, endValue.vy, t), + MathUtil.lerp(this.omega, endValue.omega, t)); } } From f159365bb43d0ec7702b8cfc8dc0fd4169f46228 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 27 Sep 2025 13:55:07 -0400 Subject: [PATCH 30/82] refactor: add ForEachNested method and override getNested in SwerveModuleAccelerationsStruct Signed-off-by: Zach Harel --- .../kinematics/struct/SwerveModuleAccelerationsStruct.java | 5 +++++ .../frc/kinematics/struct/SwerveModuleAccelerationsStruct.h | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java index 0bf011e7062..72370927c68 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleAccelerationsStruct.java @@ -30,6 +30,11 @@ public String getSchema() { return "double acceleration;Rotation2d angle"; } + @Override + public Struct[] getNested() { + return new Struct[] {Rotation2d.struct}; + } + @Override public SwerveModuleAccelerations unpack(ByteBuffer bb) { double acceleration = bb.getDouble(); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h index 85ca32e936f..fc13bae70e6 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h @@ -24,6 +24,11 @@ struct WPILIB_DLLEXPORT wpi::Struct { static frc::SwerveModuleAccelerations Unpack(std::span data); static void Pack(std::span data, const frc::SwerveModuleAccelerations& value); + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + } }; static_assert(wpi::StructSerializable); +static_assert(wpi::HasNestedStruct); From 2062c8f2b05c89d1670d995761010db3cefcdf71 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 27 Sep 2025 14:19:57 -0400 Subject: [PATCH 31/82] refactor: enhance SwerveDriveKinematicsTest with detailed acceleration calculations and determine expected values programmatically assertions Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematicsTest.java | 152 ++++++++++++++---- 1 file changed, 124 insertions(+), 28 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 724704fc7ab..ba7b66e83e2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -11,6 +11,8 @@ import edu.wpi.first.math.geometry.Translation2d; import org.junit.jupiter.api.Test; +import java.util.Arrays; + class SwerveDriveKinematicsTest { private static final double kEpsilon = 1E-9; @@ -419,24 +421,68 @@ void testTurnInPlaceInverseAccelerations() { each module has both tangential acceleration (from angular acceleration) and centripetal acceleration (from angular velocity). The total acceleration magnitude is approximately 678.4. - The acceleration angles are not simply tangential (as in pure rotation) because the - centripetal component from the existing angular velocity affects the direction: - FL: -144°, FR: 126°, BL: -54°, BR: 36° + For each swerve module at position (x, y) relative to the robot center: + - Distance from center: r = √(x² + y²) = √(12² + 12²) = 16.97 m + - Current tangential velocity: v_t = ω × r = 2π × 16.97 = 106.63 m/s + + Two acceleration components: + 1) Tangential acceleration (from angular acceleration α = 2π rad/s²): + a_tangential = α × r = 2π × 16.97 = 106.63 m/s² + Direction: perpendicular to radius vector + + 2) Centripetal acceleration (from angular velocity ω = 2π rad/s): + a_centripetal = ω² × r = (2π)² × 16.97 = 668.7 m/s² + Direction: toward center of rotation - These angles reflect the combination of: - - Tangential acceleration from angular acceleration (2π rad/s²) - - Centripetal acceleration from angular velocity (2π rad/s) + Total acceleration magnitude: |a_total| = √(a_tangential² + a_centripetal²) + = √(106.63² + 668.7²) = 678.4 m/s² + + For module positions: + FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = -144° + FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = 126° + BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = -54° + BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = 36° + + The acceleration angles are not simply tangential because the centripetal component + from the existing angular velocity dominates and affects the direction. */ + double centerRadius = m_kinematics.getModules()[0].getNorm(); + double tangentialAccel = centerRadius * accelerations.alpha; // α * r + double centripetalAccel = centerRadius * angularVelocity * angularVelocity; // ω² * r + double totalAccel = Math.hypot(tangentialAccel, centripetalAccel); + + Rotation2d[] expectedAngles = Arrays.stream(m_kinematics.getModules()) + .map(m -> { + Rotation2d radiusAngle = m.getAngle(); + + // Tangential acceleration: perpendicular to radius (90° CCW from radius) + Rotation2d tangentialDirection = radiusAngle.rotateBy(Rotation2d.kCCW_Pi_2); + double tangentialX = tangentialAccel * tangentialDirection.getCos(); + double tangentialY = tangentialAccel * tangentialDirection.getSin(); + + // Centripetal acceleration: toward center (opposite of radius) + Rotation2d centripetalDirection = radiusAngle.rotateBy(Rotation2d.kPi); + double centripetalX = centripetalAccel * centripetalDirection.getCos(); + double centripetalY = centripetalAccel * centripetalDirection.getSin(); + + // Vector sum of tangential and centripetal accelerations + double totalX = tangentialX + centripetalX; + double totalY = tangentialY + centripetalY; + + return new Rotation2d(totalX, totalY); + }) + .toArray(Rotation2d[]::new); + assertAll( - () -> assertEquals(678.4, moduleAccelerations[0].acceleration, 0.1), - () -> assertEquals(678.4, moduleAccelerations[1].acceleration, 0.1), - () -> assertEquals(678.4, moduleAccelerations[2].acceleration, 0.1), - () -> assertEquals(678.4, moduleAccelerations[3].acceleration, 0.1), - () -> assertEquals(-144.0, moduleAccelerations[0].angle.getDegrees(), 0.1), - () -> assertEquals(126.0, moduleAccelerations[1].angle.getDegrees(), 0.1), - () -> assertEquals(-54.0, moduleAccelerations[2].angle.getDegrees(), 0.1), - () -> assertEquals(36.0, moduleAccelerations[3].angle.getDegrees(), 0.1)); + () -> assertEquals(totalAccel, moduleAccelerations[0].acceleration, 0.1), + () -> assertEquals(totalAccel, moduleAccelerations[1].acceleration, 0.1), + () -> assertEquals(totalAccel, moduleAccelerations[2].acceleration, 0.1), + () -> assertEquals(totalAccel, moduleAccelerations[3].acceleration, 0.1), + () -> assertEquals(expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), 0.1), + () -> assertEquals(expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), 0.1), + () -> assertEquals(expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), 0.1), + () -> assertEquals(expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), 0.1)); } @Test @@ -472,23 +518,73 @@ void testOffCenterRotationInverseAccelerations() { and angular velocity (1 rad/s), each module experiences both tangential and centripetal accelerations that combine vectorially. - FL: at center of rotation (0 acceleration) - FR: 24 units from center, experiences combined acceleration → ~33.94 - BL: 24 units from center, experiences combined acceleration → ~33.94 - BR: ~33.94 units from center, experiences combined acceleration → ~48.0 + Center of rotation: FL module at (12, 12) inches + Module positions relative to center of rotation: + - FL: (0, 0) - at center of rotation + - FR: (0, -24) - 24 m south of center + - BL: (-24, 0) - 24 m west of center + - BR: (-24, -24) - distance = √(24² + 24²) = 33.94 m southwest - Angles reflect the vector combination of tangential and centripetal components: - FL: -45° (though magnitude is 0), FR: 45°, BL: -45°, BR: 0° + For each module at distance r from center of rotation: + 1) Tangential acceleration: a_t = α × r = 1 × r + Direction: perpendicular to radius vector (90° CCW from radius) + + 2) Centripetal acceleration: a_c = ω² × r = 1² × r = r + Direction: toward center of rotation */ + double[] expectedAccelerations = Arrays.stream(m_kinematics.getModules()) + .mapToDouble(m -> { + Translation2d relativePos = m.minus(m_fl); + double r = relativePos.getNorm(); + + if (r < 1e-9) { + return 0.0; // No acceleration at center of rotation + } + + double tangentialAccel = r * accelerations.alpha; // α * r = 1 * r + double centripetalAccel = r * angularVelocity * angularVelocity; // ω² * r = 1 * r + return Math.hypot(tangentialAccel, centripetalAccel); + }) + .toArray(); + + Rotation2d[] expectedAngles = Arrays.stream(m_kinematics.getModules()) + .map(m -> { + Translation2d relativePos = m.minus(m_fl); + double r = relativePos.getNorm(); + + if (r < 1e-9) { + return Rotation2d.kZero; // Angle is undefined at center of rotation + } + + Rotation2d radiusAngle = new Rotation2d(relativePos.getX(), relativePos.getY()); + + // Tangential acceleration: perpendicular to radius (90° CCW from radius) + Rotation2d tangentialDirection = radiusAngle.rotateBy(Rotation2d.kCCW_Pi_2); + double tangentialX = tangentialDirection.getCos() * r; // α * r = 1 * r + double tangentialY = tangentialDirection.getSin() * r; + + // Centripetal acceleration: toward center (opposite of radius) + Rotation2d centripetalDirection = radiusAngle.rotateBy(Rotation2d.kPi); + double centripetalX = centripetalDirection.getCos() * r; // ω² * r = 1 * r + double centripetalY = centripetalDirection.getSin() * r; + + // Vector sum of tangential and centripetal accelerations + double totalX = tangentialX + centripetalX; + double totalY = tangentialY + centripetalY; + + return new Rotation2d(totalX, totalY); + }) + .toArray(Rotation2d[]::new); + assertAll( - () -> assertEquals(0, moduleAccelerations[0].acceleration, 0.1), - () -> assertEquals(33.94, moduleAccelerations[1].acceleration, 0.1), - () -> assertEquals(33.94, moduleAccelerations[2].acceleration, 0.1), - () -> assertEquals(48.0, moduleAccelerations[3].acceleration, 0.1), - () -> assertEquals(0.0, moduleAccelerations[0].angle.getDegrees(), 0.1), - () -> assertEquals(45.0, moduleAccelerations[1].angle.getDegrees(), 0.1), - () -> assertEquals(-45.0, moduleAccelerations[2].angle.getDegrees(), 0.1), - () -> assertEquals(0.0, moduleAccelerations[3].angle.getDegrees(), 0.1)); + () -> assertEquals(expectedAccelerations[0], moduleAccelerations[0].acceleration, 0.1), + () -> assertEquals(expectedAccelerations[1], moduleAccelerations[1].acceleration, 0.1), + () -> assertEquals(expectedAccelerations[2], moduleAccelerations[2].acceleration, 0.1), + () -> assertEquals(expectedAccelerations[3], moduleAccelerations[3].acceleration, 0.1), + () -> assertEquals(expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), 0.1), + () -> assertEquals(expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), 0.1), + () -> assertEquals(expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), 0.1), + () -> assertEquals(expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), 0.1)); } } From 41922e91018bfd7bfdda73ac7e8ed1efc53c7c85 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 27 Sep 2025 14:52:57 -0400 Subject: [PATCH 32/82] refactor: make gradle happy Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematicsTest.java | 161 ++++++++++-------- 1 file changed, 91 insertions(+), 70 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index ba7b66e83e2..cd714c52460 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -9,9 +9,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import org.junit.jupiter.api.Test; - import java.util.Arrays; +import org.junit.jupiter.api.Test; class SwerveDriveKinematicsTest { private static final double kEpsilon = 1E-9; @@ -452,37 +451,47 @@ For each swerve module at position (x, y) relative to the robot center: double centripetalAccel = centerRadius * angularVelocity * angularVelocity; // ω² * r double totalAccel = Math.hypot(tangentialAccel, centripetalAccel); - Rotation2d[] expectedAngles = Arrays.stream(m_kinematics.getModules()) - .map(m -> { - Rotation2d radiusAngle = m.getAngle(); + Rotation2d[] expectedAngles = + Arrays.stream(m_kinematics.getModules()) + .map( + m -> { + Rotation2d radiusAngle = m.getAngle(); - // Tangential acceleration: perpendicular to radius (90° CCW from radius) - Rotation2d tangentialDirection = radiusAngle.rotateBy(Rotation2d.kCCW_Pi_2); - double tangentialX = tangentialAccel * tangentialDirection.getCos(); - double tangentialY = tangentialAccel * tangentialDirection.getSin(); + // Tangential acceleration: perpendicular to radius (90° CCW from radius) + Rotation2d tangentialDirection = radiusAngle.rotateBy(Rotation2d.kCCW_Pi_2); + double tangentialX = tangentialAccel * tangentialDirection.getCos(); + double tangentialY = tangentialAccel * tangentialDirection.getSin(); - // Centripetal acceleration: toward center (opposite of radius) - Rotation2d centripetalDirection = radiusAngle.rotateBy(Rotation2d.kPi); - double centripetalX = centripetalAccel * centripetalDirection.getCos(); - double centripetalY = centripetalAccel * centripetalDirection.getSin(); + // Centripetal acceleration: toward center (opposite of radius) + Rotation2d centripetalDirection = radiusAngle.rotateBy(Rotation2d.kPi); + double centripetalX = centripetalAccel * centripetalDirection.getCos(); + double centripetalY = centripetalAccel * centripetalDirection.getSin(); - // Vector sum of tangential and centripetal accelerations - double totalX = tangentialX + centripetalX; - double totalY = tangentialY + centripetalY; + // Vector sum of tangential and centripetal accelerations + double totalX = tangentialX + centripetalX; + double totalY = tangentialY + centripetalY; - return new Rotation2d(totalX, totalY); - }) - .toArray(Rotation2d[]::new); + return new Rotation2d(totalX, totalY); + }) + .toArray(Rotation2d[]::new); assertAll( () -> assertEquals(totalAccel, moduleAccelerations[0].acceleration, 0.1), () -> assertEquals(totalAccel, moduleAccelerations[1].acceleration, 0.1), () -> assertEquals(totalAccel, moduleAccelerations[2].acceleration, 0.1), () -> assertEquals(totalAccel, moduleAccelerations[3].acceleration, 0.1), - () -> assertEquals(expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), 0.1), - () -> assertEquals(expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), 0.1), - () -> assertEquals(expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), 0.1), - () -> assertEquals(expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), 0.1)); + () -> + assertEquals( + expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), 0.1), + () -> + assertEquals( + expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), 0.1), + () -> + assertEquals( + expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), 0.1), + () -> + assertEquals( + expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), 0.1)); } @Test @@ -533,58 +542,70 @@ and angular velocity (1 rad/s), each module experiences both tangential and cent Direction: toward center of rotation */ - double[] expectedAccelerations = Arrays.stream(m_kinematics.getModules()) - .mapToDouble(m -> { - Translation2d relativePos = m.minus(m_fl); - double r = relativePos.getNorm(); - - if (r < 1e-9) { - return 0.0; // No acceleration at center of rotation - } - - double tangentialAccel = r * accelerations.alpha; // α * r = 1 * r - double centripetalAccel = r * angularVelocity * angularVelocity; // ω² * r = 1 * r - return Math.hypot(tangentialAccel, centripetalAccel); - }) - .toArray(); - - Rotation2d[] expectedAngles = Arrays.stream(m_kinematics.getModules()) - .map(m -> { - Translation2d relativePos = m.minus(m_fl); - double r = relativePos.getNorm(); - - if (r < 1e-9) { - return Rotation2d.kZero; // Angle is undefined at center of rotation - } - - Rotation2d radiusAngle = new Rotation2d(relativePos.getX(), relativePos.getY()); - - // Tangential acceleration: perpendicular to radius (90° CCW from radius) - Rotation2d tangentialDirection = radiusAngle.rotateBy(Rotation2d.kCCW_Pi_2); - double tangentialX = tangentialDirection.getCos() * r; // α * r = 1 * r - double tangentialY = tangentialDirection.getSin() * r; - - // Centripetal acceleration: toward center (opposite of radius) - Rotation2d centripetalDirection = radiusAngle.rotateBy(Rotation2d.kPi); - double centripetalX = centripetalDirection.getCos() * r; // ω² * r = 1 * r - double centripetalY = centripetalDirection.getSin() * r; - - // Vector sum of tangential and centripetal accelerations - double totalX = tangentialX + centripetalX; - double totalY = tangentialY + centripetalY; - - return new Rotation2d(totalX, totalY); - }) - .toArray(Rotation2d[]::new); + double[] expectedAccelerations = + Arrays.stream(m_kinematics.getModules()) + .mapToDouble( + m -> { + Translation2d relativePos = m.minus(m_fl); + double r = relativePos.getNorm(); + + if (r < 1e-9) { + return 0.0; // No acceleration at center of rotation + } + + double tangentialAccel = r * accelerations.alpha; // α * r = 1 * r + double centripetalAccel = r * angularVelocity * angularVelocity; // ω² * r = 1 * r + return Math.hypot(tangentialAccel, centripetalAccel); + }) + .toArray(); + + Rotation2d[] expectedAngles = + Arrays.stream(m_kinematics.getModules()) + .map( + m -> { + Translation2d relativePos = m.minus(m_fl); + double r = relativePos.getNorm(); + + if (r < 1e-9) { + return Rotation2d.kZero; // Angle is undefined at center of rotation + } + + Rotation2d radiusAngle = new Rotation2d(relativePos.getX(), relativePos.getY()); + + // Tangential acceleration: perpendicular to radius (90° CCW from radius) + Rotation2d tangentialDirection = radiusAngle.rotateBy(Rotation2d.kCCW_Pi_2); + double tangentialX = tangentialDirection.getCos() * r; // α * r = 1 * r + double tangentialY = tangentialDirection.getSin() * r; + + // Centripetal acceleration: toward center (opposite of radius) + Rotation2d centripetalDirection = radiusAngle.rotateBy(Rotation2d.kPi); + double centripetalX = centripetalDirection.getCos() * r; // ω² * r = 1 * r + double centripetalY = centripetalDirection.getSin() * r; + + // Vector sum of tangential and centripetal accelerations + double totalX = tangentialX + centripetalX; + double totalY = tangentialY + centripetalY; + + return new Rotation2d(totalX, totalY); + }) + .toArray(Rotation2d[]::new); assertAll( () -> assertEquals(expectedAccelerations[0], moduleAccelerations[0].acceleration, 0.1), () -> assertEquals(expectedAccelerations[1], moduleAccelerations[1].acceleration, 0.1), () -> assertEquals(expectedAccelerations[2], moduleAccelerations[2].acceleration, 0.1), () -> assertEquals(expectedAccelerations[3], moduleAccelerations[3].acceleration, 0.1), - () -> assertEquals(expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), 0.1), - () -> assertEquals(expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), 0.1), - () -> assertEquals(expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), 0.1), - () -> assertEquals(expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), 0.1)); + () -> + assertEquals( + expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), 0.1), + () -> + assertEquals( + expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), 0.1), + () -> + assertEquals( + expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), 0.1), + () -> + assertEquals( + expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), 0.1)); } } From e633c8cf6dc54dfea7bf7d4b1c903fbcae3fd45b Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 27 Sep 2025 14:56:39 -0400 Subject: [PATCH 33/82] test: add c++ unit tests for Chassis, DifferentialDrive, Mecanum, and Swerve module accelerations Signed-off-by: Zach Harel --- .../kinematics/ChassisAccelerationsTest.cpp | 100 ++++++++++++++++++ ...ifferentialDriveWheelAccelerationsTest.cpp | 67 ++++++++++++ .../MecanumDriveWheelAccelerationsTest.cpp | 84 +++++++++++++++ .../SwerveModuleAccelerationsTest.cpp | 37 +++++++ 4 files changed, 288 insertions(+) create mode 100644 wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp create mode 100644 wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp create mode 100644 wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp create mode 100644 wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp new file mode 100644 index 00000000000..99d3147dbd0 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp @@ -0,0 +1,100 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include + +#include "frc/kinematics/ChassisAccelerations.h" +#include "units/acceleration.h" +#include "units/angular_acceleration.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; + +TEST(ChassisAccelerationsTest, DefaultConstructor) { + ChassisAccelerations accelerations; + + EXPECT_NEAR(accelerations.ax.value(), 0.0, kEpsilon); + EXPECT_NEAR(accelerations.ay.value(), 0.0, kEpsilon); + EXPECT_NEAR(accelerations.alpha.value(), 0.0, kEpsilon); +} + +TEST(ChassisAccelerationsTest, ParameterizedConstructor) { + ChassisAccelerations accelerations{1.0_mps_sq, 2.0_mps_sq, 3.0_rad_per_s_sq}; + + EXPECT_NEAR(accelerations.ax.value(), 1.0, kEpsilon); + EXPECT_NEAR(accelerations.ay.value(), 2.0, kEpsilon); + EXPECT_NEAR(accelerations.alpha.value(), 3.0, kEpsilon); +} + +TEST(ChassisAccelerationsTest, ToRobotRelative) { + const auto chassisAccelerations = + ChassisAccelerations{1.0_mps_sq, 0.0_mps_sq, 0.5_rad_per_s_sq} + .ToRobotRelative(Rotation2d{-90_deg}); + + EXPECT_NEAR(chassisAccelerations.ax.value(), 0.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), 1.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), 0.5, kEpsilon); +} + +TEST(ChassisAccelerationsTest, ToFieldRelative) { + const auto chassisAccelerations = + ChassisAccelerations{1.0_mps_sq, 0.0_mps_sq, 0.5_rad_per_s_sq} + .ToFieldRelative(Rotation2d{45_deg}); + + EXPECT_NEAR(chassisAccelerations.ax.value(), 1.0 / std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), 1.0 / std::sqrt(2.0), kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), 0.5, kEpsilon); +} + +TEST(ChassisAccelerationsTest, Plus) { + const ChassisAccelerations left{1.0_mps_sq, 0.5_mps_sq, 0.75_rad_per_s_sq}; + const ChassisAccelerations right{2.0_mps_sq, 1.5_mps_sq, 0.25_rad_per_s_sq}; + + const auto chassisAccelerations = left + right; + + EXPECT_NEAR(chassisAccelerations.ax.value(), 3.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), 2.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), 1.0, kEpsilon); +} + +TEST(ChassisAccelerationsTest, Minus) { + const ChassisAccelerations left{1.0_mps_sq, 0.5_mps_sq, 0.75_rad_per_s_sq}; + const ChassisAccelerations right{2.0_mps_sq, 0.5_mps_sq, 0.25_rad_per_s_sq}; + + const auto chassisAccelerations = left - right; + + EXPECT_NEAR(chassisAccelerations.ax.value(), -1.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), 0.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), 0.5, kEpsilon); +} + +TEST(ChassisAccelerationsTest, UnaryMinus) { + const auto chassisAccelerations = + -ChassisAccelerations{1.0_mps_sq, 0.5_mps_sq, 0.75_rad_per_s_sq}; + + EXPECT_NEAR(chassisAccelerations.ax.value(), -1.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), -0.5, kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), -0.75, kEpsilon); +} + +TEST(ChassisAccelerationsTest, Multiplication) { + const auto chassisAccelerations = + ChassisAccelerations{1.0_mps_sq, 0.5_mps_sq, 0.75_rad_per_s_sq} * 2.0; + + EXPECT_NEAR(chassisAccelerations.ax.value(), 2.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), 1.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), 1.5, kEpsilon); +} + +TEST(ChassisAccelerationsTest, Division) { + const auto chassisAccelerations = + ChassisAccelerations{2.0_mps_sq, 1.0_mps_sq, 1.5_rad_per_s_sq} / 2.0; + + EXPECT_NEAR(chassisAccelerations.ax.value(), 1.0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), 0.5, kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), 0.75, kEpsilon); +} \ No newline at end of file diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp new file mode 100644 index 00000000000..f2b4327e544 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp @@ -0,0 +1,67 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/kinematics/DifferentialDriveWheelAccelerations.h" +#include "units/acceleration.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; + +TEST(DifferentialDriveWheelAccelerationsTest, DefaultConstructor) { + DifferentialDriveWheelAccelerations wheelAccelerations; + + EXPECT_NEAR(wheelAccelerations.left.value(), 0.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), 0.0, kEpsilon); +} + +TEST(DifferentialDriveWheelAccelerationsTest, ParameterizedConstructor) { + DifferentialDriveWheelAccelerations wheelAccelerations{1.5_mps_sq, 2.5_mps_sq}; + + EXPECT_NEAR(wheelAccelerations.left.value(), 1.5, kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), 2.5, kEpsilon); +} + +TEST(DifferentialDriveWheelAccelerationsTest, Plus) { + const DifferentialDriveWheelAccelerations left{1.0_mps_sq, 0.5_mps_sq}; + const DifferentialDriveWheelAccelerations right{2.0_mps_sq, 1.5_mps_sq}; + + const auto wheelAccelerations = left + right; + + EXPECT_NEAR(wheelAccelerations.left.value(), 3.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), 2.0, kEpsilon); +} + +TEST(DifferentialDriveWheelAccelerationsTest, Minus) { + const DifferentialDriveWheelAccelerations left{1.0_mps_sq, 0.5_mps_sq}; + const DifferentialDriveWheelAccelerations right{2.0_mps_sq, 0.5_mps_sq}; + + const auto wheelAccelerations = left - right; + + EXPECT_NEAR(wheelAccelerations.left.value(), -1.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), 0.0, kEpsilon); +} + +TEST(DifferentialDriveWheelAccelerationsTest, UnaryMinus) { + const auto wheelAccelerations = -DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq}; + + EXPECT_NEAR(wheelAccelerations.left.value(), -1.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), -0.5, kEpsilon); +} + +TEST(DifferentialDriveWheelAccelerationsTest, Multiplication) { + const auto wheelAccelerations = DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq} * 2.0; + + EXPECT_NEAR(wheelAccelerations.left.value(), 2.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), 1.0, kEpsilon); +} + +TEST(DifferentialDriveWheelAccelerationsTest, Division) { + const auto wheelAccelerations = DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq} / 2.0; + + EXPECT_NEAR(wheelAccelerations.left.value(), 0.5, kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), 0.25, kEpsilon); +} \ No newline at end of file diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp new file mode 100644 index 00000000000..c905283b027 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp @@ -0,0 +1,84 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/kinematics/MecanumDriveWheelAccelerations.h" +#include "units/acceleration.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; + +TEST(MecanumDriveWheelAccelerationsTest, DefaultConstructor) { + MecanumDriveWheelAccelerations wheelAccelerations; + + EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 0.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.frontRight.value(), 0.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 0.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearRight.value(), 0.0, kEpsilon); +} + +TEST(MecanumDriveWheelAccelerationsTest, ParameterizedConstructor) { + MecanumDriveWheelAccelerations wheelAccelerations{1.0_mps_sq, 2.0_mps_sq, 3.0_mps_sq, 4.0_mps_sq}; + + EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 1.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 3.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearRight.value(), 4.0, kEpsilon); +} + +TEST(MecanumDriveWheelAccelerationsTest, Plus) { + const MecanumDriveWheelAccelerations left{1.0_mps_sq, 0.5_mps_sq, 2.0_mps_sq, 1.5_mps_sq}; + const MecanumDriveWheelAccelerations right{2.0_mps_sq, 1.5_mps_sq, 0.5_mps_sq, 1.0_mps_sq}; + + const auto wheelAccelerations = left + right; + + EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 3.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 2.5, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearRight.value(), 2.5, kEpsilon); +} + +TEST(MecanumDriveWheelAccelerationsTest, Minus) { + const MecanumDriveWheelAccelerations left{5.0_mps_sq, 4.0_mps_sq, 6.0_mps_sq, 2.5_mps_sq}; + const MecanumDriveWheelAccelerations right{1.0_mps_sq, 2.0_mps_sq, 3.0_mps_sq, 0.5_mps_sq}; + + const auto wheelAccelerations = left - right; + + EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 4.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 3.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearRight.value(), 2.0, kEpsilon); +} + +TEST(MecanumDriveWheelAccelerationsTest, UnaryMinus) { + const auto wheelAccelerations = + -MecanumDriveWheelAccelerations{1.0_mps_sq, -2.0_mps_sq, 3.0_mps_sq, -4.0_mps_sq}; + + EXPECT_NEAR(wheelAccelerations.frontLeft.value(), -1.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearLeft.value(), -3.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearRight.value(), 4.0, kEpsilon); +} + +TEST(MecanumDriveWheelAccelerationsTest, Multiplication) { + const auto wheelAccelerations = + MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 3.0_mps_sq, 3.5_mps_sq} * 2.0; + + EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 4.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.frontRight.value(), 5.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 6.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearRight.value(), 7.0, kEpsilon); +} + +TEST(MecanumDriveWheelAccelerationsTest, Division) { + const auto wheelAccelerations = + MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 1.5_mps_sq, 1.0_mps_sq} / 2.0; + + EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 1.0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.frontRight.value(), 1.25, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 0.75, kEpsilon); + EXPECT_NEAR(wheelAccelerations.rearRight.value(), 0.5, kEpsilon); +} \ No newline at end of file diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp new file mode 100644 index 00000000000..a0d2a74e26f --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include + +#include "frc/kinematics/SwerveModuleAccelerations.h" +#include "units/acceleration.h" + +using namespace frc; + +static constexpr double kEpsilon = 1E-9; + +TEST(SwerveModuleAccelerationsTest, DefaultConstructor) { + SwerveModuleAccelerations moduleAccelerations; + + EXPECT_NEAR(moduleAccelerations.acceleration.value(), 0.0, kEpsilon); + EXPECT_NEAR(moduleAccelerations.angle.Radians().value(), 0.0, kEpsilon); +} + +TEST(SwerveModuleAccelerationsTest, ParameterizedConstructor) { + SwerveModuleAccelerations moduleAccelerations{2.5_mps_sq, Rotation2d{1.5_rad}}; + + EXPECT_NEAR(moduleAccelerations.acceleration.value(), 2.5, kEpsilon); + EXPECT_NEAR(moduleAccelerations.angle.Radians().value(), 1.5, kEpsilon); +} + +TEST(SwerveModuleAccelerationsTest, Equals) { + SwerveModuleAccelerations moduleAccelerations1{2.0_mps_sq, Rotation2d{1.5_rad}}; + SwerveModuleAccelerations moduleAccelerations2{2.0_mps_sq, Rotation2d{1.5_rad}}; + SwerveModuleAccelerations moduleAccelerations3{2.1_mps_sq, Rotation2d{1.5_rad}}; + + EXPECT_EQ(moduleAccelerations1, moduleAccelerations2); + EXPECT_NE(moduleAccelerations1, moduleAccelerations3); +} From ce2f91280f16723e9bfe70cfbc49d697ab234487 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 27 Sep 2025 15:09:38 -0400 Subject: [PATCH 34/82] refactor: use single line comments for test derivations Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematicsTest.java | 95 +++++++++---------- 1 file changed, 46 insertions(+), 49 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index cd714c52460..187a7544c7e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -415,36 +415,35 @@ void testTurnInPlaceInverseAccelerations() { var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity); - /* - For turn-in-place with angular acceleration of 2π rad/s² and angular velocity of 2π rad/s, - each module has both tangential acceleration (from angular acceleration) and centripetal - acceleration (from angular velocity). The total acceleration magnitude is approximately 678.4. - - For each swerve module at position (x, y) relative to the robot center: - - Distance from center: r = √(x² + y²) = √(12² + 12²) = 16.97 m - - Current tangential velocity: v_t = ω × r = 2π × 16.97 = 106.63 m/s - - Two acceleration components: - 1) Tangential acceleration (from angular acceleration α = 2π rad/s²): - a_tangential = α × r = 2π × 16.97 = 106.63 m/s² - Direction: perpendicular to radius vector - - 2) Centripetal acceleration (from angular velocity ω = 2π rad/s): - a_centripetal = ω² × r = (2π)² × 16.97 = 668.7 m/s² - Direction: toward center of rotation - - Total acceleration magnitude: |a_total| = √(a_tangential² + a_centripetal²) - = √(106.63² + 668.7²) = 678.4 m/s² - - For module positions: - FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = -144° - FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = 126° - BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = -54° - BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = 36° - - The acceleration angles are not simply tangential because the centripetal component - from the existing angular velocity dominates and affects the direction. - */ + // For turn-in-place with angular acceleration of 2π rad/s² and angular velocity of 2π rad/s, + // each module has both tangential acceleration (from angular acceleration) and centripetal + // acceleration (from angular velocity). The total acceleration magnitude is approximately 678.4. + // + // For each swerve module at position (x, y) relative to the robot center: + // - Distance from center: r = √(x² + y²) = √(12² + 12²) = 16.97 m + // - Current tangential velocity: v_t = ω × r = 2π × 16.97 = 106.63 m/s + // + // Two acceleration components: + // 1) Tangential acceleration (from angular acceleration α = 2π rad/s²): + // a_tangential = α × r = 2π × 16.97 = 106.63 m/s² + // Direction: perpendicular to radius vector + // + // 2) Centripetal acceleration (from angular velocity ω = 2π rad/s): + // a_centripetal = ω² × r = (2π)² × 16.97 = 668.7 m/s² + // Direction: toward center of rotation + // + // Total acceleration magnitude: |a_total| = √(a_tangential² + a_centripetal²) + // = √(106.63² + 668.7²) = 678.4 m/s² + // + // For module positions: + // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = -144° + // FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = 126° + // BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = -54° + // BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = 36° + // + // The acceleration angles are not simply tangential because the centripetal component + // from the existing angular velocity dominates and affects the direction. + // double centerRadius = m_kinematics.getModules()[0].getNorm(); double tangentialAccel = centerRadius * accelerations.alpha; // α * r @@ -522,25 +521,23 @@ void testOffCenterRotationInverseAccelerations() { var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity, m_fl); - /* - When rotating about the front-left module position with both angular acceleration (1 rad/s²) - and angular velocity (1 rad/s), each module experiences both tangential and centripetal - accelerations that combine vectorially. - - Center of rotation: FL module at (12, 12) inches - Module positions relative to center of rotation: - - FL: (0, 0) - at center of rotation - - FR: (0, -24) - 24 m south of center - - BL: (-24, 0) - 24 m west of center - - BR: (-24, -24) - distance = √(24² + 24²) = 33.94 m southwest - - For each module at distance r from center of rotation: - 1) Tangential acceleration: a_t = α × r = 1 × r - Direction: perpendicular to radius vector (90° CCW from radius) - - 2) Centripetal acceleration: a_c = ω² × r = 1² × r = r - Direction: toward center of rotation - */ + // When rotating about the front-left module position with both angular acceleration (1 rad/s²) + // and angular velocity (1 rad/s), each module experiences both tangential and centripetal + // accelerations that combine vectorially. + // + // Center of rotation: FL module at (12, 12) inches + // Module positions relative to center of rotation: + // - FL: (0, 0) - at center of rotation + // - FR: (0, -24) - 24 m south of center + // - BL: (-24, 0) - 24 m west of center + // - BR: (-24, -24) - distance = √(24² + 24²) = 33.94 m southwest + // + // For each module at distance r from center of rotation: + // 1) Tangential acceleration: a_t = α × r = 1 × r + // Direction: perpendicular to radius vector (90° CCW from radius) + // + // 2) Centripetal acceleration: a_c = ω² × r = 1² × r = r + // Direction: toward center of rotation double[] expectedAccelerations = Arrays.stream(m_kinematics.getModules()) From 35d175c1c73f73365f0e359923348de143b38f22 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 27 Sep 2025 15:09:45 -0400 Subject: [PATCH 35/82] wpiformat aka make gradle happy Signed-off-by: Zach Harel --- .../struct/SwerveModuleAccelerationsStruct.h | 2 +- .../kinematics/ChassisAccelerationsTest.cpp | 2 +- ...ifferentialDriveWheelAccelerationsTest.cpp | 14 +++++---- .../MecanumDriveWheelAccelerationsTest.cpp | 29 ++++++++++++------- .../SwerveModuleAccelerationsTest.cpp | 12 +++++--- 5 files changed, 38 insertions(+), 21 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h index fc13bae70e6..a7599d0d90c 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleAccelerationsStruct.h @@ -25,7 +25,7 @@ struct WPILIB_DLLEXPORT wpi::Struct { static void Pack(std::span data, const frc::SwerveModuleAccelerations& value); static void ForEachNested( - std::invocable auto fn) { + std::invocable auto fn) { wpi::ForEachStructSchema(fn); } }; diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp index 99d3147dbd0..5470df8d8cb 100644 --- a/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp @@ -97,4 +97,4 @@ TEST(ChassisAccelerationsTest, Division) { EXPECT_NEAR(chassisAccelerations.ax.value(), 1.0, kEpsilon); EXPECT_NEAR(chassisAccelerations.ay.value(), 0.5, kEpsilon); EXPECT_NEAR(chassisAccelerations.alpha.value(), 0.75, kEpsilon); -} \ No newline at end of file +} diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp index f2b4327e544..4be9485aaf9 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp @@ -19,7 +19,8 @@ TEST(DifferentialDriveWheelAccelerationsTest, DefaultConstructor) { } TEST(DifferentialDriveWheelAccelerationsTest, ParameterizedConstructor) { - DifferentialDriveWheelAccelerations wheelAccelerations{1.5_mps_sq, 2.5_mps_sq}; + DifferentialDriveWheelAccelerations wheelAccelerations{1.5_mps_sq, + 2.5_mps_sq}; EXPECT_NEAR(wheelAccelerations.left.value(), 1.5, kEpsilon); EXPECT_NEAR(wheelAccelerations.right.value(), 2.5, kEpsilon); @@ -46,22 +47,25 @@ TEST(DifferentialDriveWheelAccelerationsTest, Minus) { } TEST(DifferentialDriveWheelAccelerationsTest, UnaryMinus) { - const auto wheelAccelerations = -DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq}; + const auto wheelAccelerations = + -DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq}; EXPECT_NEAR(wheelAccelerations.left.value(), -1.0, kEpsilon); EXPECT_NEAR(wheelAccelerations.right.value(), -0.5, kEpsilon); } TEST(DifferentialDriveWheelAccelerationsTest, Multiplication) { - const auto wheelAccelerations = DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq} * 2.0; + const auto wheelAccelerations = + DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq} * 2.0; EXPECT_NEAR(wheelAccelerations.left.value(), 2.0, kEpsilon); EXPECT_NEAR(wheelAccelerations.right.value(), 1.0, kEpsilon); } TEST(DifferentialDriveWheelAccelerationsTest, Division) { - const auto wheelAccelerations = DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq} / 2.0; + const auto wheelAccelerations = + DifferentialDriveWheelAccelerations{1.0_mps_sq, 0.5_mps_sq} / 2.0; EXPECT_NEAR(wheelAccelerations.left.value(), 0.5, kEpsilon); EXPECT_NEAR(wheelAccelerations.right.value(), 0.25, kEpsilon); -} \ No newline at end of file +} diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp index c905283b027..32c94221e68 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp @@ -21,7 +21,8 @@ TEST(MecanumDriveWheelAccelerationsTest, DefaultConstructor) { } TEST(MecanumDriveWheelAccelerationsTest, ParameterizedConstructor) { - MecanumDriveWheelAccelerations wheelAccelerations{1.0_mps_sq, 2.0_mps_sq, 3.0_mps_sq, 4.0_mps_sq}; + MecanumDriveWheelAccelerations wheelAccelerations{1.0_mps_sq, 2.0_mps_sq, + 3.0_mps_sq, 4.0_mps_sq}; EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 1.0, kEpsilon); EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon); @@ -30,8 +31,10 @@ TEST(MecanumDriveWheelAccelerationsTest, ParameterizedConstructor) { } TEST(MecanumDriveWheelAccelerationsTest, Plus) { - const MecanumDriveWheelAccelerations left{1.0_mps_sq, 0.5_mps_sq, 2.0_mps_sq, 1.5_mps_sq}; - const MecanumDriveWheelAccelerations right{2.0_mps_sq, 1.5_mps_sq, 0.5_mps_sq, 1.0_mps_sq}; + const MecanumDriveWheelAccelerations left{1.0_mps_sq, 0.5_mps_sq, 2.0_mps_sq, + 1.5_mps_sq}; + const MecanumDriveWheelAccelerations right{2.0_mps_sq, 1.5_mps_sq, 0.5_mps_sq, + 1.0_mps_sq}; const auto wheelAccelerations = left + right; @@ -42,8 +45,10 @@ TEST(MecanumDriveWheelAccelerationsTest, Plus) { } TEST(MecanumDriveWheelAccelerationsTest, Minus) { - const MecanumDriveWheelAccelerations left{5.0_mps_sq, 4.0_mps_sq, 6.0_mps_sq, 2.5_mps_sq}; - const MecanumDriveWheelAccelerations right{1.0_mps_sq, 2.0_mps_sq, 3.0_mps_sq, 0.5_mps_sq}; + const MecanumDriveWheelAccelerations left{5.0_mps_sq, 4.0_mps_sq, 6.0_mps_sq, + 2.5_mps_sq}; + const MecanumDriveWheelAccelerations right{1.0_mps_sq, 2.0_mps_sq, 3.0_mps_sq, + 0.5_mps_sq}; const auto wheelAccelerations = left - right; @@ -54,8 +59,8 @@ TEST(MecanumDriveWheelAccelerationsTest, Minus) { } TEST(MecanumDriveWheelAccelerationsTest, UnaryMinus) { - const auto wheelAccelerations = - -MecanumDriveWheelAccelerations{1.0_mps_sq, -2.0_mps_sq, 3.0_mps_sq, -4.0_mps_sq}; + const auto wheelAccelerations = -MecanumDriveWheelAccelerations{ + 1.0_mps_sq, -2.0_mps_sq, 3.0_mps_sq, -4.0_mps_sq}; EXPECT_NEAR(wheelAccelerations.frontLeft.value(), -1.0, kEpsilon); EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon); @@ -65,7 +70,9 @@ TEST(MecanumDriveWheelAccelerationsTest, UnaryMinus) { TEST(MecanumDriveWheelAccelerationsTest, Multiplication) { const auto wheelAccelerations = - MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 3.0_mps_sq, 3.5_mps_sq} * 2.0; + MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 3.0_mps_sq, + 3.5_mps_sq} * + 2.0; EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 4.0, kEpsilon); EXPECT_NEAR(wheelAccelerations.frontRight.value(), 5.0, kEpsilon); @@ -75,10 +82,12 @@ TEST(MecanumDriveWheelAccelerationsTest, Multiplication) { TEST(MecanumDriveWheelAccelerationsTest, Division) { const auto wheelAccelerations = - MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 1.5_mps_sq, 1.0_mps_sq} / 2.0; + MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 1.5_mps_sq, + 1.0_mps_sq} / + 2.0; EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 1.0, kEpsilon); EXPECT_NEAR(wheelAccelerations.frontRight.value(), 1.25, kEpsilon); EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 0.75, kEpsilon); EXPECT_NEAR(wheelAccelerations.rearRight.value(), 0.5, kEpsilon); -} \ No newline at end of file +} diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp index a0d2a74e26f..386070fd8e4 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp @@ -21,16 +21,20 @@ TEST(SwerveModuleAccelerationsTest, DefaultConstructor) { } TEST(SwerveModuleAccelerationsTest, ParameterizedConstructor) { - SwerveModuleAccelerations moduleAccelerations{2.5_mps_sq, Rotation2d{1.5_rad}}; + SwerveModuleAccelerations moduleAccelerations{2.5_mps_sq, + Rotation2d{1.5_rad}}; EXPECT_NEAR(moduleAccelerations.acceleration.value(), 2.5, kEpsilon); EXPECT_NEAR(moduleAccelerations.angle.Radians().value(), 1.5, kEpsilon); } TEST(SwerveModuleAccelerationsTest, Equals) { - SwerveModuleAccelerations moduleAccelerations1{2.0_mps_sq, Rotation2d{1.5_rad}}; - SwerveModuleAccelerations moduleAccelerations2{2.0_mps_sq, Rotation2d{1.5_rad}}; - SwerveModuleAccelerations moduleAccelerations3{2.1_mps_sq, Rotation2d{1.5_rad}}; + SwerveModuleAccelerations moduleAccelerations1{2.0_mps_sq, + Rotation2d{1.5_rad}}; + SwerveModuleAccelerations moduleAccelerations2{2.0_mps_sq, + Rotation2d{1.5_rad}}; + SwerveModuleAccelerations moduleAccelerations3{2.1_mps_sq, + Rotation2d{1.5_rad}}; EXPECT_EQ(moduleAccelerations1, moduleAccelerations2); EXPECT_NE(moduleAccelerations1, moduleAccelerations3); From 3ec8179cdea812671defa9a50e713f8e047726d4 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 27 Sep 2025 15:25:47 -0400 Subject: [PATCH 36/82] make ~~gradle~~ the comment police happy Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematicsTest.java | 153 ++++++++---------- 1 file changed, 70 insertions(+), 83 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 187a7544c7e..6a4f3ec677b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -147,12 +147,10 @@ void testTurnInPlaceInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); var moduleStates = m_kinematics.toSwerveModuleStates(speeds); - /* - The circumference of the wheels about the COR is π * diameter, or 2π * radius - the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels - trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second, - our wheels must trace out 1 rotation (or 106.63 inches) per second. - */ + // The circumference of the wheels about the COR is π * diameter, or 2π * radius + // the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels + // trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second, + // our wheels must trace out 1 rotation (or 106.63 inches) per second. assertAll( () -> assertEquals(106.63, moduleStates[0].speed, 0.1), @@ -200,14 +198,12 @@ void testOffCenterCORRotationInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl); - /* - This one is a bit trickier. Because we are rotating about the front-left wheel, - it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel - an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with - radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel - should be pointing straight forward, the back-left wheel should be pointing straight right, - and the back-right wheel should be at a -45 degree angle - */ + // This one is a bit trickier. Because we are rotating about the front-left wheel, + // it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel + // an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with + // radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel + // should be pointing straight forward, the back-left wheel should be pointing straight right, + // and the back-right wheel should be at a -45 degree angle assertAll( () -> assertEquals(0.0, moduleStates[0].speed, 0.1), @@ -229,14 +225,12 @@ void testOffCenterCORRotationForwardKinematics() { var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState); - /* - We already know that our omega should be 2π from the previous test. Next, we need to determine - the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, - we know that vx and vy must be the same. Furthermore, we know that the center of mass makes - a full revolution about the center of revolution once every second. Therefore, the center of - mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are - 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. - */ + // We already know that our omega should be 2π from the previous test. Next, we need to determine + // the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, + // we know that vx and vy must be the same. Furthermore, we know that the center of mass makes + // a full revolution about the center of revolution once every second. Therefore, the center of + // mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are + // 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. assertAll( () -> assertEquals(75.398, chassisSpeeds.vx, 0.1), @@ -253,14 +247,12 @@ void testOffCenterCORRotationForwardKinematicsWithDeltas() { var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); - /* - We already know that our omega should be 2π from the previous test. Next, we need to determine - the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, - we know that vx and vy must be the same. Furthermore, we know that the center of mass makes - a full revolution about the center of revolution once every second. Therefore, the center of - mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are - 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. - */ + // We already know that our omega should be 2π from the previous test. Next, we need to determine + // the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, + // we know that vx and vy must be the same. Furthermore, we know that the center of mass makes + // a full revolution about the center of revolution once every second. Therefore, the center of + // mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are + // 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. assertAll( () -> assertEquals(75.398, twist.dx, 0.1), @@ -313,14 +305,12 @@ void testOffCenterCORRotationAndTranslationForwardKinematics() { var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState); - /* - From equation (13.17), we know that chassis motion is th dot product of the - pseudoinverse of the inverseKinematics matrix (with the center of rotation at - (0,0) -- we don't want the motion of the center of rotation, we want it of - the center of the robot). These above SwerveModuleStates are known to be from - a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been - calculated using Numpy's linalg.pinv function. - */ + // From equation (13.17), we know that chassis motion is th dot product of the + // pseudoinverse of the inverseKinematics matrix (with the center of rotation at + // (0,0) -- we don't want the motion of the center of rotation, we want it of + // the center of the robot). These above SwerveModuleStates are known to be from + // a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been + // calculated using Numpy's linalg.pinv function. assertAll( () -> assertEquals(0.0, chassisSpeeds.vx, 0.1), @@ -337,14 +327,12 @@ void testOffCenterCORRotationAndTranslationForwardKinematicsWithDeltas() { var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); - /* - From equation (13.17), we know that chassis motion is th dot product of the - pseudoinverse of the inverseKinematics matrix (with the center of rotation at - (0,0) -- we don't want the motion of the center of rotation, we want it of - the center of the robot). These above SwerveModuleStates are known to be from - a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been - calculated using Numpy's linalg.pinv function. - */ + // From equation (13.17), we know that chassis motion is th dot product of the + // pseudoinverse of the inverseKinematics matrix (with the center of rotation at + // (0,0) -- we don't want the motion of the center of rotation, we want it of + // the center of the robot). These above SwerveModuleStates are known to be from + // a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been + // calculated using Numpy's linalg.pinv function. assertAll( () -> assertEquals(0.0, twist.dx, 0.1), @@ -415,35 +403,34 @@ void testTurnInPlaceInverseAccelerations() { var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity); - // For turn-in-place with angular acceleration of 2π rad/s² and angular velocity of 2π rad/s, - // each module has both tangential acceleration (from angular acceleration) and centripetal - // acceleration (from angular velocity). The total acceleration magnitude is approximately 678.4. + // For turn-in-place with angular acceleration of 2π rad/s² and angular velocity of 2π rad/s, + // each module has both tangential acceleration (from angular acceleration) and centripetal + // acceleration (from angular velocity). The total acceleration magnitude is approximately 678.4. // - // For each swerve module at position (x, y) relative to the robot center: - // - Distance from center: r = √(x² + y²) = √(12² + 12²) = 16.97 m - // - Current tangential velocity: v_t = ω × r = 2π × 16.97 = 106.63 m/s + // For each swerve module at position (x, y) relative to the robot center: + // - Distance from center: r = √(x² + y²) = √(12² + 12²) = 16.97 m + // - Current tangential velocity: v_t = ω × r = 2π × 16.97 = 106.63 m/s // - // Two acceleration components: - // 1) Tangential acceleration (from angular acceleration α = 2π rad/s²): - // a_tangential = α × r = 2π × 16.97 = 106.63 m/s² - // Direction: perpendicular to radius vector + // Two acceleration components: + // 1) Tangential acceleration (from angular acceleration α = 2π rad/s²): + // a_tangential = α × r = 2π × 16.97 = 106.63 m/s² + // Direction: perpendicular to radius vector // - // 2) Centripetal acceleration (from angular velocity ω = 2π rad/s): - // a_centripetal = ω² × r = (2π)² × 16.97 = 668.7 m/s² - // Direction: toward center of rotation + // 2) Centripetal acceleration (from angular velocity ω = 2π rad/s): + // a_centripetal = ω² × r = (2π)² × 16.97 = 668.7 m/s² + // Direction: toward center of rotation // - // Total acceleration magnitude: |a_total| = √(a_tangential² + a_centripetal²) - // = √(106.63² + 668.7²) = 678.4 m/s² + // Total acceleration magnitude: |a_total| = √(a_tangential² + a_centripetal²) + // = √(106.63² + 668.7²) = 678.4 m/s² // - // For module positions: - // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = -144° - // FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = 126° - // BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = -54° - // BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = 36° - // - // The acceleration angles are not simply tangential because the centripetal component - // from the existing angular velocity dominates and affects the direction. + // For module positions: + // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = -144° + // FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = 126° + // BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = -54° + // BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = 36° // + // The acceleration angles are not simply tangential because the centripetal component + // from the existing angular velocity dominates and affects the direction. double centerRadius = m_kinematics.getModules()[0].getNorm(); double tangentialAccel = centerRadius * accelerations.alpha; // α * r @@ -521,23 +508,23 @@ void testOffCenterRotationInverseAccelerations() { var moduleAccelerations = m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity, m_fl); - // When rotating about the front-left module position with both angular acceleration (1 rad/s²) - // and angular velocity (1 rad/s), each module experiences both tangential and centripetal - // accelerations that combine vectorially. + // When rotating about the front-left module position with both angular acceleration (1 rad/s²) + // and angular velocity (1 rad/s), each module experiences both tangential and centripetal + // accelerations that combine vectorially. // - // Center of rotation: FL module at (12, 12) inches - // Module positions relative to center of rotation: - // - FL: (0, 0) - at center of rotation - // - FR: (0, -24) - 24 m south of center - // - BL: (-24, 0) - 24 m west of center - // - BR: (-24, -24) - distance = √(24² + 24²) = 33.94 m southwest + // Center of rotation: FL module at (12, 12) inches + // Module positions relative to center of rotation: + // - FL: (0, 0) - at center of rotation + // - FR: (0, -24) - 24 m south of center + // - BL: (-24, 0) - 24 m west of center + // - BR: (-24, -24) - distance = √(24² + 24²) = 33.94 m southwest // - // For each module at distance r from center of rotation: - // 1) Tangential acceleration: a_t = α × r = 1 × r - // Direction: perpendicular to radius vector (90° CCW from radius) + // For each module at distance r from center of rotation: + // 1) Tangential acceleration: a_t = α × r = 1 × r + // Direction: perpendicular to radius vector (90° CCW from radius) // - // 2) Centripetal acceleration: a_c = ω² × r = 1² × r = r - // Direction: toward center of rotation + // 2) Centripetal acceleration: a_c = ω² × r = 1² × r = r + // Direction: toward center of rotation double[] expectedAccelerations = Arrays.stream(m_kinematics.getModules()) From ce9a0a312f65227a2ded7c6ae728dfa62b1aa65a Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 27 Sep 2025 15:41:18 -0400 Subject: [PATCH 37/82] make gradle happy Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematicsTest.java | 27 ++++++++++++------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 6a4f3ec677b..dfed9478045 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -199,8 +199,10 @@ void testOffCenterCORRotationInverseKinematics() { var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl); // This one is a bit trickier. Because we are rotating about the front-left wheel, - // it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel - // an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with + // it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both + // travel + // an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc + // with // radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel // should be pointing straight forward, the back-left wheel should be pointing straight right, // and the back-right wheel should be at a -45 degree angle @@ -225,7 +227,8 @@ void testOffCenterCORRotationForwardKinematics() { var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState); - // We already know that our omega should be 2π from the previous test. Next, we need to determine + // We already know that our omega should be 2π from the previous test. Next, we need to + // determine // the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, // we know that vx and vy must be the same. Furthermore, we know that the center of mass makes // a full revolution about the center of revolution once every second. Therefore, the center of @@ -247,7 +250,8 @@ void testOffCenterCORRotationForwardKinematicsWithDeltas() { var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); - // We already know that our omega should be 2π from the previous test. Next, we need to determine + // We already know that our omega should be 2π from the previous test. Next, we need to + // determine // the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, // we know that vx and vy must be the same. Furthermore, we know that the center of mass makes // a full revolution about the center of revolution once every second. Therefore, the center of @@ -405,7 +409,8 @@ void testTurnInPlaceInverseAccelerations() { // For turn-in-place with angular acceleration of 2π rad/s² and angular velocity of 2π rad/s, // each module has both tangential acceleration (from angular acceleration) and centripetal - // acceleration (from angular velocity). The total acceleration magnitude is approximately 678.4. + // acceleration (from angular velocity). The total acceleration magnitude is approximately + // 678.4. // // For each swerve module at position (x, y) relative to the robot center: // - Distance from center: r = √(x² + y²) = √(12² + 12²) = 16.97 m @@ -424,10 +429,14 @@ void testTurnInPlaceInverseAccelerations() { // = √(106.63² + 668.7²) = 678.4 m/s² // // For module positions: - // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = -144° - // FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = 126° - // BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = -54° - // BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = 36° + // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = + // -144° + // FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = + // 126° + // BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = + // -54° + // BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = + // 36° // // The acceleration angles are not simply tangential because the centripetal component // from the existing angular velocity dominates and affects the direction. From 9b36359fe1dbf0deafad9df3f8bbb36f1aa7e60d Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 10 Oct 2025 17:28:20 -0400 Subject: [PATCH 38/82] feat: add methods for converting between chassis and wheel accelerations to Kinematics.h Signed-off-by: Zach Harel --- .../include/frc/kinematics/Kinematics.h | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/Kinematics.h b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h index 3901630e590..3fb80e487ff 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Kinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h @@ -7,6 +7,7 @@ #include #include "frc/geometry/Twist2d.h" +#include "frc/kinematics/ChassisAccelerations.h" #include "frc/kinematics/ChassisSpeeds.h" namespace frc { @@ -19,7 +20,8 @@ namespace frc { * Inverse kinematics converts a desired chassis speed into wheel speeds whereas * forward kinematics converts wheel speeds into chassis speed. */ -template +template requires std::copy_constructible && std::assignable_from class WPILIB_DLLEXPORT Kinematics { @@ -75,5 +77,29 @@ class WPILIB_DLLEXPORT Kinematics { virtual WheelPositions Interpolate(const WheelPositions& start, const WheelPositions& end, double t) const = 0; + + /** + * Performs forward kinematics to return the resulting chassis accelerations + * from the wheel accelerations. This method is often used for dynamics + * calculations -- determining the robot's acceleration on the field using + * data from the real-world acceleration of each wheel on the robot. + * + * @param wheelAccelerations The accelerations of the wheels. + * @return The chassis accelerations. + */ + virtual ChassisAccelerations ToChassisAccelerations( + const WheelAccelerations& wheelAccelerations) const = 0; + + /** + * Performs inverse kinematics to return the wheel accelerations from a + * desired chassis acceleration. This method is often used for dynamics + * calculations -- converting desired robot accelerations into individual + * wheel accelerations. + * + * @param chassisAccelerations The desired chassis accelerations. + * @return The wheel accelerations. + */ + virtual WheelAccelerations ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations) const = 0; }; } // namespace frc From 32cfe2e227584e21a0288c3622fb4198f2d5d92f Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 10 Oct 2025 17:32:57 -0400 Subject: [PATCH 39/82] feat: add methods for converting between chassis and wheel accelerations in DifferentialDriveKinematics.h Signed-off-by: Zach Harel --- .../kinematics/DifferentialDriveKinematics.h | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index 517fdf50ac5..d6c64416e50 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -8,6 +8,7 @@ #include +#include "DifferentialDriveWheelAccelerations.h" #include "frc/geometry/Twist2d.h" #include "frc/kinematics/ChassisSpeeds.h" #include "frc/kinematics/DifferentialDriveWheelPositions.h" @@ -28,7 +29,8 @@ namespace frc { */ class WPILIB_DLLEXPORT DifferentialDriveKinematics : public Kinematics { + DifferentialDriveWheelPositions, + DifferentialDriveWheelAccelerations> { public: /** * Constructs a differential drive kinematics object. @@ -99,6 +101,23 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics return start.Interpolate(end, t); } + constexpr ChassisAccelerations ToChassisAccelerations( + const DifferentialDriveWheelAccelerations& wheelAccelerations) + const override { + return {(wheelAccelerations.left + wheelAccelerations.right) / 2.0, + 0_mps_sq, + (wheelAccelerations.right - wheelAccelerations.left) / trackwidth * + 1_rad}; + } + + constexpr DifferentialDriveWheelAccelerations ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations) const override { + return {chassisAccelerations.ax - + trackwidth / 2 * chassisAccelerations.alpha / 1_rad, + chassisAccelerations.ax + + trackwidth / 2 * chassisAccelerations.alpha / 1_rad}; + } + /// Differential drive trackwidth. units::meter_t trackwidth; }; From 43793f64f6697cbe6978c5800f13d3456ddb72e4 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 10 Oct 2025 17:42:24 -0400 Subject: [PATCH 40/82] feat: add methods for converting between chassis and wheel accelerations in MecanumDriveKinematics c++ Signed-off-by: Zach Harel --- .../cpp/kinematics/MecanumDriveKinematics.cpp | 44 +++++++++++++++++++ .../frc/kinematics/MecanumDriveKinematics.h | 16 ++++++- 2 files changed, 59 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index c21a7b20341..970bb9e718c 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -85,3 +85,47 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl, {1, 1, (rl.X() - rl.Y()).value()}, {1, -1, (-(rr.X() + rr.Y())).value()}}; } + +ChassisAccelerations MecanumDriveKinematics::ToChassisAccelerations( + const MecanumDriveWheelAccelerations& wheelAccelerations) const { + Eigen::Vector4d wheelSpeedsVector{wheelAccelerations.frontLeft.value(), + wheelAccelerations.frontRight.value(), + wheelAccelerations.rearLeft.value(), + wheelAccelerations.rearRight.value()}; + + Eigen::Vector3d chassisSpeedsVector = + m_forwardKinematics.solve(wheelSpeedsVector); + + return {units::meters_per_second_squared_t{chassisSpeedsVector(0)}, // NOLINT + units::meters_per_second_squared_t{chassisSpeedsVector(1)}, + units::radians_per_second_squared_t{chassisSpeedsVector(2)}}; +} + +MecanumDriveWheelAccelerations MecanumDriveKinematics::ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations, + const Translation2d& centerOfRotation) const { + // We have a new center of rotation. We need to compute the matrix again. + if (centerOfRotation != m_previousCoR) { + auto fl = m_frontLeftWheel - centerOfRotation; + auto fr = m_frontRightWheel - centerOfRotation; + auto rl = m_rearLeftWheel - centerOfRotation; + auto rr = m_rearRightWheel - centerOfRotation; + + SetInverseKinematics(fl, fr, rl, rr); + + m_previousCoR = centerOfRotation; + } + + Eigen::Vector3d chassisSpeedsVector{chassisAccelerations.ax.value(), + chassisAccelerations.ay.value(), + chassisAccelerations.alpha.value()}; + + Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector; + + MecanumDriveWheelAccelerations wheelAccels; + wheelAccels.frontLeft = units::meters_per_second_squared_t{wheelsVector(0)}; + wheelAccels.frontRight = units::meters_per_second_squared_t{wheelsVector(1)}; + wheelAccels.rearLeft = units::meters_per_second_squared_t{wheelsVector(2)}; + wheelAccels.rearRight = units::meters_per_second_squared_t{wheelsVector(3)}; + return wheelAccels; +} diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index e33eb5ccc99..605d4e34e04 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -7,6 +7,7 @@ #include #include +#include "MecanumDriveWheelAccelerations.h" #include "frc/EigenCore.h" #include "frc/geometry/Translation2d.h" #include "frc/geometry/Twist2d.h" @@ -41,7 +42,8 @@ namespace frc { * the robot on the field using encoders and a gyro. */ class WPILIB_DLLEXPORT MecanumDriveKinematics - : public Kinematics { + : public Kinematics { public: /** * Constructs a mecanum drive kinematics object. @@ -195,6 +197,18 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics */ void SetInverseKinematics(Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) const; + + ChassisAccelerations ToChassisAccelerations( + const MecanumDriveWheelAccelerations& wheelAccelerations) const override; + + MecanumDriveWheelAccelerations ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations, + const Translation2d& centerOfRotation) const; + + MecanumDriveWheelAccelerations ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations) const override { + return ToWheelAccelerations(chassisAccelerations, {}); + } }; } // namespace frc From 9395b4723d6d3ad2e0c96ec3d2a626d235134254 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 10:55:07 -0400 Subject: [PATCH 41/82] fix: update include for DifferentialDriveWheelAccelerations in DifferentialDriveKinematics.h Signed-off-by: Zach Harel --- .../native/include/frc/kinematics/DifferentialDriveKinematics.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index d6c64416e50..230ebf8411a 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -8,9 +8,9 @@ #include -#include "DifferentialDriveWheelAccelerations.h" #include "frc/geometry/Twist2d.h" #include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/DifferentialDriveWheelAccelerations.h" #include "frc/kinematics/DifferentialDriveWheelPositions.h" #include "frc/kinematics/DifferentialDriveWheelSpeeds.h" #include "frc/kinematics/Kinematics.h" From 1302ddeefff6081ce00fa6454c6464ed6f0469c4 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 10:55:15 -0400 Subject: [PATCH 42/82] feat: add support for module accelerations in SwerveDriveKinematics.h Signed-off-by: Zach Harel --- .../frc/kinematics/SwerveDriveKinematics.h | 226 ++++++++++++++++-- 1 file changed, 207 insertions(+), 19 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 2aa2a885236..d16406e8f3a 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -18,6 +18,7 @@ #include "frc/geometry/Twist2d.h" #include "frc/kinematics/ChassisSpeeds.h" #include "frc/kinematics/Kinematics.h" +#include "frc/kinematics/SwerveModuleAccelerations.h" #include "frc/kinematics/SwerveModulePosition.h" #include "frc/kinematics/SwerveModuleState.h" #include "units/math.h" @@ -51,7 +52,8 @@ namespace frc { template class SwerveDriveKinematics : public Kinematics, - wpi::array> { + wpi::array, + wpi::array> { public: /** * Constructs a swerve drive kinematics object. This takes in a variable @@ -70,13 +72,20 @@ class SwerveDriveKinematics : m_modules{moduleTranslations...}, m_moduleHeadings(wpi::empty_array) { for (size_t i = 0; i < NumModules; i++) { // clang-format off - m_inverseKinematics.template block<2, 3>(i * 2, 0) << + m_firstOrderInverseKinematics.template block<2, 3>(i * 2, 0) << 1, 0, (-m_modules[i].Y()).value(), 0, 1, (+m_modules[i].X()).value(); + + m_secondOrderInverseKinematics.template block<2, 4>(i * 2, 0) << + 1, 0, (-m_modules[i].X()).value(), (-m_modules[i].Y()).value(), + 0, 1, (-m_modules[i].Y()).value(), (+m_modules[i].X()).value(); // clang-format on } - m_forwardKinematics = m_inverseKinematics.householderQr(); + m_firstOrderForwardKinematics = + m_firstOrderInverseKinematics.householderQr(); + m_secondOrderForwardKinematics = + m_secondOrderInverseKinematics.householderQr(); wpi::math::MathSharedStore::ReportUsage("SwerveDriveKinematics", ""); } @@ -86,13 +95,20 @@ class SwerveDriveKinematics : m_modules{modules}, m_moduleHeadings(wpi::empty_array) { for (size_t i = 0; i < NumModules; i++) { // clang-format off - m_inverseKinematics.template block<2, 3>(i * 2, 0) << + m_firstOrderInverseKinematics.template block<2, 3>(i * 2, 0) << 1, 0, (-m_modules[i].Y()).value(), 0, 1, (+m_modules[i].X()).value(); + + m_secondOrderInverseKinematics.template block<2, 4>(i * 2, 0) << + 1, 0, (-m_modules[i].X()).value(), (-m_modules[i].Y()).value(), + 0, 1, (-m_modules[i].Y()).value(), (+m_modules[i].X()).value(); // clang-format on } - m_forwardKinematics = m_inverseKinematics.householderQr(); + m_firstOrderForwardKinematics = + m_firstOrderInverseKinematics.householderQr(); + m_secondOrderForwardKinematics = + m_secondOrderInverseKinematics.householderQr(); wpi::math::MathSharedStore::ReportUsage("Kinematics_SwerveDrive", ""); } @@ -169,15 +185,7 @@ class SwerveDriveKinematics // We have a new center of rotation. We need to compute the matrix again. if (centerOfRotation != m_previousCoR) { - for (size_t i = 0; i < NumModules; i++) { - // clang-format off - m_inverseKinematics.template block<2, 3>(i * 2, 0) = - Matrixd<2, 3>{ - {1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()}, - {0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}}; - // clang-format on - } - m_previousCoR = centerOfRotation; + setInverseKinematics(centerOfRotation); } Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(), @@ -185,7 +193,7 @@ class SwerveDriveKinematics chassisSpeeds.omega.value()}; Matrixd moduleStateMatrix = - m_inverseKinematics * chassisSpeedsVector; + m_firstOrderInverseKinematics * chassisSpeedsVector; for (size_t i = 0; i < NumModules; i++) { units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)}; @@ -251,7 +259,7 @@ class SwerveDriveKinematics } Eigen::Vector3d chassisSpeedsVector = - m_forwardKinematics.solve(moduleStateMatrix); + m_firstOrderForwardKinematics.solve(moduleStateMatrix); return {units::meters_per_second_t{chassisSpeedsVector(0)}, units::meters_per_second_t{chassisSpeedsVector(1)}, @@ -304,7 +312,7 @@ class SwerveDriveKinematics } Eigen::Vector3d chassisDeltaVector = - m_forwardKinematics.solve(moduleDeltaMatrix); + m_firstOrderForwardKinematics.solve(moduleDeltaMatrix); return {units::meter_t{chassisDeltaVector(0)}, units::meter_t{chassisDeltaVector(1)}, @@ -441,13 +449,193 @@ class SwerveDriveKinematics return m_modules; } + /** + * Performs inverse kinematics to return the module accelerations from a + * desired chassis acceleration. This method is often used for dynamics + * calculations -- converting desired robot accelerations into individual + * module accelerations. + * + *

This function also supports variable centers of rotation. During normal + * operations, the center of rotation is usually the same as the physical + * center of the robot; therefore, the argument is defaulted to that use case. + * However, if you wish to change the center of rotation for evasive + * maneuvers, vision alignment, or for any other use case, you can do so. + * + * @param chassisAccelerations The desired chassis accelerations. + * @param angularVelocity The desired robot angular velocity. + * @param centerOfRotation The center of rotation. For example, if you set the + * center of rotation at one corner of the robot and provide a chassis + * acceleration that only has a dtheta component, the robot will rotate around + * that corner. + * @return An array containing the module accelerations. + */ + wpi::array ToSwerveModuleAccelerations( + const ChassisAccelerations& chassisAccelerations, + const units::radians_per_second_t angularVelocity = 0.0_rad_per_s, + const Translation2d& centerOfRotation = Translation2d{}) const { + wpi::array moduleAccelerations( + wpi::empty_array); + + if (chassisAccelerations.ax == 0.0_mps_sq && + chassisAccelerations.ay == 0.0_mps_sq && + chassisAccelerations.alpha == 0.0_rad_per_s_sq) { + for (int i = 0; i < NumModules; i++) { + moduleAccelerations[i] = { + 0.0_mps_sq, Rotation2d{0.0, 0.0}}; // maintain previous angle + } + return moduleAccelerations; + } + + if (centerOfRotation != m_previousCoR) { + setInverseKinematics(centerOfRotation); + } + + Eigen::Vector4d chassisAccelerationsVector{ + chassisAccelerations.ax.value(), chassisAccelerations.ay.value(), + angularVelocity.value() * angularVelocity.value(), + chassisAccelerations.alpha.value()}; + + Matrixd moduleAccelerationsMatrix = + m_secondOrderInverseKinematics * chassisAccelerationsVector; + + for (int i = 0; i < NumModules; i++) { + units::meters_per_second_squared_t x{ + moduleAccelerationsMatrix.get(i * 2, 0)}; + units::meters_per_second_squared_t y{ + moduleAccelerationsMatrix.get(i * 2 + 1, 0)}; + + // For swerve modules, we need to compute both linear acceleration and + // angular acceleration The linear acceleration is the magnitude of the + // acceleration vector + units::meters_per_second_squared_t linearAcceleration = + units::math::hypot(x, y); + + moduleAccelerations[i] = {linearAcceleration, + Rotation2d{(x.value()), y.value()}}; + } + + return moduleAccelerations; + } + + /** + * Performs inverse kinematics to return the module accelerations from a + * desired chassis acceleration. This method is often used for dynamics + * calculations -- converting desired robot accelerations into individual + * module accelerations. + * + * @param chassisAccelerations The desired chassis accelerations. + * @param angularVelocity The desired robot angular velocity. + * @return An array containing the module accelerations. + */ + wpi::array ToSwerveModuleAccelerations( + const ChassisAccelerations& chassisAccelerations, + const units::radians_per_second_t angularVelocity) const { + return ToSwerveModuleAccelerations(chassisAccelerations, angularVelocity, + Translation2d{}); + } + + /** + * Performs inverse kinematics to return the module accelerations from a + * desired chassis acceleration. This method is often used for dynamics + * calculations -- converting desired robot accelerations into individual + * module accelerations. + * + * @param chassisAccelerations The desired chassis accelerations. + * @return An array containing the module accelerations. + */ + wpi::array ToSwerveModuleAccelerations( + const ChassisAccelerations& chassisAccelerations) const { + return ToSwerveModuleAccelerations(chassisAccelerations, 0.0_rad_per_s, + Translation2d{}); + } + + /** + * Performs inverse kinematics to return the module accelerations from a + * desired chassis acceleration. This method is often used for dynamics + * calculations -- converting desired robot accelerations into individual + * module accelerations. + * + * @param chassisAccelerations The desired chassis accelerations. + * @return An array containing the module accelerations. + */ + wpi::array ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations) const override { + return ToSwerveModuleAccelerations(chassisAccelerations, 0.0_rad_per_s, + Translation2d{}); + } + + /** + * Performs forward kinematics to return the resulting chassis accelerations + * from the given module accelerations. This method is often used for dynamics + * calculations -- determining the robot's acceleration on the field using + * data from the real-world acceleration of each module on the robot. + * + * @param moduleAccelerations The accelerations of the modules as measured + * from respective encoders and gyros. The order of the swerve module + * accelerations should be same as passed into the constructor of this class. + * @return The resulting chassis accelerations. + */ + ChassisAccelerations ToChassisAccelerations( + const wpi::array& + moduleAccelerations) const override { + Matrixd moduleAccelerationsMatrix; + + for (int i = 0; i < NumModules; i++) { + SwerveModuleAccelerations module = moduleAccelerations[i]; + + moduleAccelerationsMatrix(i * 2 + 0, 0) = + module.acceleration * module.angle.Cos(); + moduleAccelerationsMatrix(i * 2 + 1, 0) = + module.acceleration * module.angle.Sin(); + } + + Eigen::Vector4d chassisAccelerationsVector = + m_secondOrderForwardKinematics * moduleAccelerationsMatrix; + + // the second order kinematics equation for swerve drive yields a state + // vector [aₓ, a_y, ω², α] + return {units::meters_per_second_squared_t{chassisAccelerationsVector(0)}, + units::meters_per_second_squared_t{chassisAccelerationsVector(1)}, + units::radians_per_second_squared_t{chassisAccelerationsVector(3)}}; + } + private: wpi::array m_modules; - mutable Matrixd m_inverseKinematics; - Eigen::HouseholderQR> m_forwardKinematics; + mutable Matrixd m_firstOrderInverseKinematics; + Eigen::HouseholderQR> + m_firstOrderForwardKinematics; + mutable Matrixd m_secondOrderInverseKinematics; + Eigen::HouseholderQR> + m_secondOrderForwardKinematics; mutable wpi::array m_moduleHeadings; mutable Translation2d m_previousCoR; + + /** + * Sets both inverse kinematics matrices based on the new center of rotation. + * This does not check if the new center of rotation is different from the + * previous one, so a check should be included before the call to this + * function. + * + * @param centerOfRotation new center of rotation + */ + void SetInverseKinematics(const Translation2d& centerOfRotation) { + for (int i = 0; i < NumModules; i++) { + const double rx = m_modules[i].getX() - centerOfRotation.X(); + const double ry = m_modules[i].getY() - centerOfRotation.Y(); + + m_firstOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, + -ry); + m_firstOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, + rx); + + m_secondOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, + -rx, -ry); + m_secondOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, + -ry, +rx); + } + m_previousCoR = centerOfRotation; + } }; template From acfac2e6bf175820a83f240a9611cdcf6695863d Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 11:15:30 -0400 Subject: [PATCH 43/82] feat: add wheel accelerations to odometry class templates Signed-off-by: Zach Harel --- .../include/frc/kinematics/DifferentialDriveOdometry.h | 3 ++- .../frc/kinematics/DifferentialDriveOdometry3d.h | 3 ++- .../include/frc/kinematics/MecanumDriveOdometry.h | 3 ++- .../include/frc/kinematics/MecanumDriveOdometry3d.h | 3 ++- .../src/main/native/include/frc/kinematics/Odometry.h | 10 +++++++--- .../main/native/include/frc/kinematics/Odometry3d.h | 10 +++++++--- .../include/frc/kinematics/SwerveDriveOdometry.h | 3 ++- .../include/frc/kinematics/SwerveDriveOdometry3d.h | 3 ++- 8 files changed, 26 insertions(+), 12 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h index c0c406c22f1..0d9265d6d6a 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h @@ -28,7 +28,8 @@ namespace frc { */ class WPILIB_DLLEXPORT DifferentialDriveOdometry : public Odometry { + DifferentialDriveWheelPositions, + DifferentialDriveWheelAccelerations> { public: /** * Constructs a DifferentialDriveOdometry object. diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h index d6470442f27..1ba32546221 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h @@ -28,7 +28,8 @@ namespace frc { */ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d : public Odometry3d { + DifferentialDriveWheelPositions, + DifferentialDriveWheelAccelerations> { public: /** * Constructs a DifferentialDriveOdometry3d object. diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h index 0a6f537cd70..b2e7c0fc328 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h @@ -26,7 +26,8 @@ namespace frc { * when using computer-vision systems. */ class WPILIB_DLLEXPORT MecanumDriveOdometry - : public Odometry { + : public Odometry { public: /** * Constructs a MecanumDriveOdometry object. diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h index 2f3726e45de..df8e718ba0b 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h @@ -26,7 +26,8 @@ namespace frc { * when using computer-vision systems. */ class WPILIB_DLLEXPORT MecanumDriveOdometry3d - : public Odometry3d { + : public Odometry3d { public: /** * Constructs a MecanumDriveOdometry3d object. diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/wpimath/src/main/native/include/frc/kinematics/Odometry.h index 6ac4f1c958a..0fb9b9142a2 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.h @@ -25,8 +25,10 @@ namespace frc { * * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. + * @tparam WheelAccelerations Wheel accelerations type. */ -template +template class WPILIB_DLLEXPORT Odometry { public: /** @@ -37,7 +39,8 @@ class WPILIB_DLLEXPORT Odometry { * @param wheelPositions The current distances measured by each wheel. * @param initialPose The starting position of the robot on the field. */ - explicit Odometry(const Kinematics& kinematics, + explicit Odometry(const Kinematics& kinematics, const Rotation2d& gyroAngle, const WheelPositions& wheelPositions, const Pose2d& initialPose = Pose2d{}) @@ -132,7 +135,8 @@ class WPILIB_DLLEXPORT Odometry { } private: - const Kinematics& m_kinematics; + const Kinematics& + m_kinematics; Pose2d m_pose; WheelPositions m_previousWheelPositions; diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h index 0c4b819bc05..87fab2b2d80 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h @@ -28,8 +28,10 @@ namespace frc { * * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. + * @tparam WheelAccelerations Wheel accelerations type. */ -template +template class WPILIB_DLLEXPORT Odometry3d { public: /** @@ -40,7 +42,8 @@ class WPILIB_DLLEXPORT Odometry3d { * @param wheelPositions The current distances measured by each wheel. * @param initialPose The starting position of the robot on the field. */ - explicit Odometry3d(const Kinematics& kinematics, + explicit Odometry3d(const Kinematics& kinematics, const Rotation3d& gyroAngle, const WheelPositions& wheelPositions, const Pose3d& initialPose = Pose3d{}) @@ -141,7 +144,8 @@ class WPILIB_DLLEXPORT Odometry3d { } private: - const Kinematics& m_kinematics; + const Kinematics& + m_kinematics; Pose3d m_pose; WheelPositions m_previousWheelPositions; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h index f7e6c34a7bd..d4428a791ee 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h @@ -31,7 +31,8 @@ namespace frc { template class SwerveDriveOdometry : public Odometry, - wpi::array> { + wpi::array, + wpi::array> { public: /** * Constructs a SwerveDriveOdometry object. diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h index 062118582ce..f34188caff5 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h @@ -32,7 +32,8 @@ namespace frc { template class SwerveDriveOdometry3d : public Odometry3d, - wpi::array> { + wpi::array, + wpi::array> { public: /** * Constructs a SwerveDriveOdometry3d object. From 13628bb1939621324de256471259eeadddbca0e2 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 11:21:42 -0400 Subject: [PATCH 44/82] feat: extend pose estimators to include wheel accelerations Signed-off-by: Zach Harel --- .../estimator/DifferentialDrivePoseEstimator.h | 3 ++- .../estimator/DifferentialDrivePoseEstimator3d.h | 3 ++- .../frc/estimator/MecanumDrivePoseEstimator.h | 4 ++-- .../frc/estimator/MecanumDrivePoseEstimator3d.h | 3 ++- .../native/include/frc/estimator/PoseEstimator.h | 15 +++++++++------ .../include/frc/estimator/PoseEstimator3d.h | 13 ++++++++----- .../frc/estimator/SwerveDrivePoseEstimator.h | 3 ++- .../frc/estimator/SwerveDrivePoseEstimator3d.h | 6 ++++-- 8 files changed, 31 insertions(+), 19 deletions(-) diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index ccd09a5d0a9..6c305cae95e 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -32,7 +32,8 @@ namespace frc { */ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator : public PoseEstimator { + DifferentialDriveWheelPositions, + DifferentialDriveWheelAccelerations> { public: /** * Constructs a DifferentialDrivePoseEstimator with default standard diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h index 371536b2980..1adfa08fae5 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h @@ -36,7 +36,8 @@ namespace frc { */ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d : public PoseEstimator3d { + DifferentialDriveWheelPositions, + DifferentialDriveWheelAccelerations> { public: /** * Constructs a DifferentialDrivePoseEstimator3d with default standard diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index d748164f473..89e2a1c5228 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -33,8 +33,8 @@ namespace frc { * odometry. */ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator - : public PoseEstimator { + : public PoseEstimator { public: /** * Constructs a MecanumDrivePoseEstimator with default standard deviations diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h index bb1c6ea6936..1436e7b11d9 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h @@ -38,7 +38,8 @@ namespace frc { */ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d : public PoseEstimator3d { + MecanumDriveWheelPositions, + MecanumDriveWheelAccelerations> { public: /** * Constructs a MecanumDrivePoseEstimator3d with default standard deviations diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 46d124f7de7..8767140f77d 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -38,8 +38,10 @@ namespace frc { * * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. + * @tparam WheelAccelerations Wheel accelerations type. */ -template +template class WPILIB_DLLEXPORT PoseEstimator { public: /** @@ -59,10 +61,11 @@ class WPILIB_DLLEXPORT PoseEstimator { * radians). Increase these numbers to trust the vision pose measurement * less. */ - PoseEstimator(Kinematics& kinematics, - Odometry& odometry, - const wpi::array& stateStdDevs, - const wpi::array& visionMeasurementStdDevs) + PoseEstimator( + Kinematics& kinematics, + Odometry& odometry, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) : m_odometry(odometry) { for (size_t i = 0; i < 3; ++i) { m_q[i] = stateStdDevs[i] * stateStdDevs[i]; @@ -424,7 +427,7 @@ class WPILIB_DLLEXPORT PoseEstimator { static constexpr units::second_t kBufferDuration = 1.5_s; - Odometry& m_odometry; + Odometry& m_odometry; wpi::array m_q{wpi::empty_array}; Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h index 68e99dd4f2a..da932b49f45 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -44,8 +44,10 @@ namespace frc { * * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. + * @tparam WheelAccelerations Wheel accelerations type. */ -template +template class WPILIB_DLLEXPORT PoseEstimator3d { public: /** @@ -65,10 +67,11 @@ class WPILIB_DLLEXPORT PoseEstimator3d { * in meters, and angle in radians). Increase these numbers to trust the * vision pose measurement less. */ - PoseEstimator3d(Kinematics& kinematics, - Odometry3d& odometry, - const wpi::array& stateStdDevs, - const wpi::array& visionMeasurementStdDevs) + PoseEstimator3d( + Kinematics& kinematics, + Odometry3d& odometry, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) : m_odometry(odometry) { for (size_t i = 0; i < 4; ++i) { m_q[i] = stateStdDevs[i] * stateStdDevs[i]; diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index a03dde93993..369c1d305b1 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -32,7 +32,8 @@ namespace frc { template class SwerveDrivePoseEstimator : public PoseEstimator, - wpi::array> { + wpi::array, + wpi::array> { public: /** * Constructs a SwerveDrivePoseEstimator with default standard deviations diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h index 7efcbb573cd..b2948afdc2b 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h @@ -35,8 +35,10 @@ namespace frc { */ template class SwerveDrivePoseEstimator3d - : public PoseEstimator3d, - wpi::array> { + : public PoseEstimator3d< + wpi::array, + wpi::array, + wpi::array> { public: /** * Constructs a SwerveDrivePoseEstimator3d with default standard deviations From ca3479c2363acc473ae150ebb2153587cce3e8ce Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 14:22:42 -0400 Subject: [PATCH 45/82] feat: fix template issues and use eigen correctly Signed-off-by: Zach Harel --- .../include/frc/estimator/PoseEstimator3d.h | 2 +- .../frc/kinematics/SwerveDriveKinematics.h | 45 +++++++++---------- 2 files changed, 21 insertions(+), 26 deletions(-) diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h index da932b49f45..f5ea49788a1 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -438,7 +438,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { static constexpr units::second_t kBufferDuration = 1.5_s; - Odometry3d& m_odometry; + Odometry3d& m_odometry; wpi::array m_q{wpi::empty_array}; frc::Matrixd<6, 6> m_visionK = frc::Matrixd<6, 6>::Zero(); diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index d16406e8f3a..febe8cae81b 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -185,7 +185,7 @@ class SwerveDriveKinematics // We have a new center of rotation. We need to compute the matrix again. if (centerOfRotation != m_previousCoR) { - setInverseKinematics(centerOfRotation); + SetInverseKinematics(centerOfRotation); } Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(), @@ -479,7 +479,7 @@ class SwerveDriveKinematics if (chassisAccelerations.ax == 0.0_mps_sq && chassisAccelerations.ay == 0.0_mps_sq && chassisAccelerations.alpha == 0.0_rad_per_s_sq) { - for (int i = 0; i < NumModules; i++) { + for (size_t i = 0; i < NumModules; i++) { moduleAccelerations[i] = { 0.0_mps_sq, Rotation2d{0.0, 0.0}}; // maintain previous angle } @@ -487,7 +487,7 @@ class SwerveDriveKinematics } if (centerOfRotation != m_previousCoR) { - setInverseKinematics(centerOfRotation); + SetInverseKinematics(centerOfRotation); } Eigen::Vector4d chassisAccelerationsVector{ @@ -498,11 +498,10 @@ class SwerveDriveKinematics Matrixd moduleAccelerationsMatrix = m_secondOrderInverseKinematics * chassisAccelerationsVector; - for (int i = 0; i < NumModules; i++) { - units::meters_per_second_squared_t x{ - moduleAccelerationsMatrix.get(i * 2, 0)}; + for (size_t i = 0; i < NumModules; i++) { + units::meters_per_second_squared_t x{moduleAccelerationsMatrix(i * 2, 0)}; units::meters_per_second_squared_t y{ - moduleAccelerationsMatrix.get(i * 2 + 1, 0)}; + moduleAccelerationsMatrix(i * 2 + 1, 0)}; // For swerve modules, we need to compute both linear acceleration and // angular acceleration The linear acceleration is the magnitude of the @@ -580,17 +579,17 @@ class SwerveDriveKinematics moduleAccelerations) const override { Matrixd moduleAccelerationsMatrix; - for (int i = 0; i < NumModules; i++) { + for (size_t i = 0; i < NumModules; i++) { SwerveModuleAccelerations module = moduleAccelerations[i]; moduleAccelerationsMatrix(i * 2 + 0, 0) = - module.acceleration * module.angle.Cos(); + module.acceleration.value() * module.angle.Cos(); moduleAccelerationsMatrix(i * 2 + 1, 0) = - module.acceleration * module.angle.Sin(); + module.acceleration.value() * module.angle.Sin(); } Eigen::Vector4d chassisAccelerationsVector = - m_secondOrderForwardKinematics * moduleAccelerationsMatrix; + m_secondOrderForwardKinematics.solve(moduleAccelerationsMatrix); // the second order kinematics equation for swerve drive yields a state // vector [aₓ, a_y, ω², α] @@ -619,20 +618,16 @@ class SwerveDriveKinematics * * @param centerOfRotation new center of rotation */ - void SetInverseKinematics(const Translation2d& centerOfRotation) { - for (int i = 0; i < NumModules; i++) { - const double rx = m_modules[i].getX() - centerOfRotation.X(); - const double ry = m_modules[i].getY() - centerOfRotation.Y(); - - m_firstOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, - -ry); - m_firstOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, - rx); - - m_secondOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, - -rx, -ry); - m_secondOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, - -ry, +rx); + void SetInverseKinematics(const Translation2d& centerOfRotation) const { + for (size_t i = 0; i < NumModules; i++) { + const double rx = m_modules[i].X().value() - centerOfRotation.X().value(); + const double ry = m_modules[i].Y().value() - centerOfRotation.Y().value(); + + m_firstOrderInverseKinematics.row(i * 2 + 0) << 1, 0, -ry; + m_firstOrderInverseKinematics.row(i * 2 + 1) << 0, 1, rx; + + m_secondOrderInverseKinematics.row(i * 2 + 0) << 1, 0, -rx, -ry; + m_secondOrderInverseKinematics.row(i * 2 + 1) << 0, 1, -ry, +rx; } m_previousCoR = centerOfRotation; } From eab450f7f1afe923df251d8b7c3f801880ea1f50 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 14:37:30 -0400 Subject: [PATCH 46/82] feat: make acceleration methods in MecanumDriveKinematics.h public Signed-off-by: Zach Harel --- .../frc/kinematics/MecanumDriveKinematics.h | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index 605d4e34e04..d6aa0c344e1 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -173,6 +173,18 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics return start.Interpolate(end, t); } + ChassisAccelerations ToChassisAccelerations( + const MecanumDriveWheelAccelerations& wheelAccelerations) const override; + + MecanumDriveWheelAccelerations ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations, + const Translation2d& centerOfRotation) const; + + MecanumDriveWheelAccelerations ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations) const override { + return ToWheelAccelerations(chassisAccelerations, {}); + } + private: mutable Matrixd<4, 3> m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; @@ -197,18 +209,6 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics */ void SetInverseKinematics(Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) const; - - ChassisAccelerations ToChassisAccelerations( - const MecanumDriveWheelAccelerations& wheelAccelerations) const override; - - MecanumDriveWheelAccelerations ToWheelAccelerations( - const ChassisAccelerations& chassisAccelerations, - const Translation2d& centerOfRotation) const; - - MecanumDriveWheelAccelerations ToWheelAccelerations( - const ChassisAccelerations& chassisAccelerations) const override { - return ToWheelAccelerations(chassisAccelerations, {}); - } }; } // namespace frc From 1db1625bbebccdb2f171716bc88b52da53edbc01 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 14:37:53 -0400 Subject: [PATCH 47/82] feat: remove redundant ToSwerveModuleAccelerations methods and add ToChassisAccelerations overload with vararg module accels Signed-off-by: Zach Harel --- .../frc/kinematics/SwerveDriveKinematics.h | 45 +++++++------------ 1 file changed, 16 insertions(+), 29 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index febe8cae81b..f9cc040c266 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -523,44 +523,31 @@ class SwerveDriveKinematics * module accelerations. * * @param chassisAccelerations The desired chassis accelerations. - * @param angularVelocity The desired robot angular velocity. * @return An array containing the module accelerations. */ - wpi::array ToSwerveModuleAccelerations( - const ChassisAccelerations& chassisAccelerations, - const units::radians_per_second_t angularVelocity) const { - return ToSwerveModuleAccelerations(chassisAccelerations, angularVelocity, - Translation2d{}); - } - - /** - * Performs inverse kinematics to return the module accelerations from a - * desired chassis acceleration. This method is often used for dynamics - * calculations -- converting desired robot accelerations into individual - * module accelerations. - * - * @param chassisAccelerations The desired chassis accelerations. - * @return An array containing the module accelerations. - */ - wpi::array ToSwerveModuleAccelerations( - const ChassisAccelerations& chassisAccelerations) const { + wpi::array ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations) const override { return ToSwerveModuleAccelerations(chassisAccelerations, 0.0_rad_per_s, Translation2d{}); } /** - * Performs inverse kinematics to return the module accelerations from a - * desired chassis acceleration. This method is often used for dynamics - * calculations -- converting desired robot accelerations into individual - * module accelerations. + * Performs forward kinematics to return the resulting chassis accelerations + * from the given module accelerations. This method is often used for dynamics + * calculations -- determining the robot's acceleration on the field using + * data from the real-world acceleration of each module on the robot. * - * @param chassisAccelerations The desired chassis accelerations. - * @return An array containing the module accelerations. + * @param moduleAccelerations The accelerations of the modules as measured + * from respective encoders and gyros. The order of the swerve module + * accelerations should be same as passed into the constructor of this class. + * @return The resulting chassis accelerations. */ - wpi::array ToWheelAccelerations( - const ChassisAccelerations& chassisAccelerations) const override { - return ToSwerveModuleAccelerations(chassisAccelerations, 0.0_rad_per_s, - Translation2d{}); + template ... ModuleAccelerations> + requires(sizeof...(ModuleAccelerations) == NumModules) + ChassisAccelerations ToChassisAccelerations( + ModuleAccelerations&&... moduleAccelerations) const { + return this->ToChassisAccelerations( + wpi::array{moduleAccelerations...}); } /** From bc62904c885d92e280982441d52d40c8f02b9e30 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 14:38:05 -0400 Subject: [PATCH 48/82] test: port the kinematics tests from the java version Signed-off-by: Zach Harel --- .../DifferentialDriveKinematicsTest.cpp | 73 ++++++++ .../kinematics/MecanumDriveKinematicsTest.cpp | 97 ++++++++++ .../kinematics/SwerveDriveKinematicsTest.cpp | 174 ++++++++++++++++++ 3 files changed, 344 insertions(+) diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp index 782fed05db7..510d77b69d1 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp @@ -6,8 +6,10 @@ #include +#include "frc/kinematics/ChassisAccelerations.h" #include "frc/kinematics/ChassisSpeeds.h" #include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/DifferentialDriveWheelAccelerations.h" #include "units/angular_velocity.h" #include "units/length.h" #include "units/velocity.h" @@ -75,3 +77,74 @@ TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) { EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon); EXPECT_NEAR(chassisSpeeds.omega.value(), -std::numbers::pi, kEpsilon); } + +TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForZeros) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const ChassisAccelerations chassisAccelerations; + const auto wheelAccelerations = + kinematics.ToWheelAccelerations(chassisAccelerations); + + EXPECT_NEAR(wheelAccelerations.left.value(), 0, kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), 0, kEpsilon); +} + +TEST(DifferentialDriveKinematicsTest, ForwardAccelerationsForZeros) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const DifferentialDriveWheelAccelerations wheelAccelerations; + const auto chassisAccelerations = + kinematics.ToChassisAccelerations(wheelAccelerations); + + EXPECT_NEAR(chassisAccelerations.ax.value(), 0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), 0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), 0, kEpsilon); +} + +TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForStraightLine) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const ChassisAccelerations chassisAccelerations{3.0_mps_sq, 0_mps_sq, + 0_rad_per_s_sq}; + const auto wheelAccelerations = + kinematics.ToWheelAccelerations(chassisAccelerations); + + EXPECT_NEAR(wheelAccelerations.left.value(), 3, kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), 3, kEpsilon); +} + +TEST(DifferentialDriveKinematicsTest, ForwardAccelerationsForStraightLine) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const DifferentialDriveWheelAccelerations wheelAccelerations{3.0_mps_sq, + 3.0_mps_sq}; + const auto chassisAccelerations = + kinematics.ToChassisAccelerations(wheelAccelerations); + + EXPECT_NEAR(chassisAccelerations.ax.value(), 3, kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), 0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), 0, kEpsilon); +} + +TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForRotateInPlace) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const ChassisAccelerations chassisAccelerations{ + 0.0_mps_sq, 0.0_mps_sq, + units::radians_per_second_squared_t{std::numbers::pi}}; + const auto wheelAccelerations = + kinematics.ToWheelAccelerations(chassisAccelerations); + + EXPECT_NEAR(wheelAccelerations.left.value(), -0.381 * std::numbers::pi, + kEpsilon); + EXPECT_NEAR(wheelAccelerations.right.value(), +0.381 * std::numbers::pi, + kEpsilon); +} + +TEST(DifferentialDriveKinematicsTest, ForwardAccelerationsForRotateInPlace) { + const DifferentialDriveKinematics kinematics{0.381_m * 2}; + const DifferentialDriveWheelAccelerations wheelAccelerations{ + units::meters_per_second_squared_t{+0.381 * std::numbers::pi}, + units::meters_per_second_squared_t{-0.381 * std::numbers::pi}}; + const auto chassisAccelerations = + kinematics.ToChassisAccelerations(wheelAccelerations); + + EXPECT_NEAR(chassisAccelerations.ax.value(), 0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.ay.value(), 0, kEpsilon); + EXPECT_NEAR(chassisAccelerations.alpha.value(), -std::numbers::pi, kEpsilon); +} diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index 38496ba7100..ffa87692c0e 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -7,7 +7,9 @@ #include #include "frc/geometry/Translation2d.h" +#include "frc/kinematics/ChassisAccelerations.h" #include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/kinematics/MecanumDriveWheelAccelerations.h" #include "units/angular_velocity.h" using namespace frc; @@ -227,3 +229,98 @@ TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) { EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9); EXPECT_NEAR(wheelSpeeds.rearRight.value(), -7.0 * kFactor, 1E-9); } + +TEST_F(MecanumDriveKinematicsTest, StraightLineInverseAccelerations) { + ChassisAccelerations accelerations{5_mps_sq, 0_mps_sq, 0_rad_per_s_sq}; + auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations); + + EXPECT_NEAR(5.0, wheelAccelerations.frontLeft.value(), 0.1); + EXPECT_NEAR(5.0, wheelAccelerations.frontRight.value(), 0.1); + EXPECT_NEAR(5.0, wheelAccelerations.rearLeft.value(), 0.1); + EXPECT_NEAR(5.0, wheelAccelerations.rearRight.value(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, StraightLineForwardAccelerations) { + MecanumDriveWheelAccelerations wheelAccelerations{3.536_mps_sq, 3.536_mps_sq, + 3.536_mps_sq, 3.536_mps_sq}; + auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); + + EXPECT_NEAR(3.536, chassisAccelerations.ax.value(), 0.1); + EXPECT_NEAR(0, chassisAccelerations.ay.value(), 0.1); + EXPECT_NEAR(0, chassisAccelerations.alpha.value(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, StrafeInverseAccelerations) { + ChassisAccelerations accelerations{0_mps_sq, 4_mps_sq, 0_rad_per_s_sq}; + auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations); + + EXPECT_NEAR(-4.0, wheelAccelerations.frontLeft.value(), 0.1); + EXPECT_NEAR(4.0, wheelAccelerations.frontRight.value(), 0.1); + EXPECT_NEAR(4.0, wheelAccelerations.rearLeft.value(), 0.1); + EXPECT_NEAR(-4.0, wheelAccelerations.rearRight.value(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, StrafeForwardAccelerations) { + MecanumDriveWheelAccelerations wheelAccelerations{ + -2.828427_mps_sq, 2.828427_mps_sq, 2.828427_mps_sq, -2.828427_mps_sq}; + auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); + + EXPECT_NEAR(0, chassisAccelerations.ax.value(), 0.1); + EXPECT_NEAR(2.8284, chassisAccelerations.ay.value(), 0.1); + EXPECT_NEAR(0, chassisAccelerations.alpha.value(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, RotationInverseAccelerations) { + ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, + units::radians_per_second_squared_t{ + 2 * std::numbers::pi}}; + auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations); + + EXPECT_NEAR(-150.79645, wheelAccelerations.frontLeft.value(), 0.1); + EXPECT_NEAR(150.79645, wheelAccelerations.frontRight.value(), 0.1); + EXPECT_NEAR(-150.79645, wheelAccelerations.rearLeft.value(), 0.1); + EXPECT_NEAR(150.79645, wheelAccelerations.rearRight.value(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, RotationForwardAccelerations) { + MecanumDriveWheelAccelerations wheelAccelerations{ + -150.79645_mps_sq, 150.79645_mps_sq, -150.79645_mps_sq, 150.79645_mps_sq}; + auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); + + EXPECT_NEAR(0, chassisAccelerations.ax.value(), 0.1); + EXPECT_NEAR(0, chassisAccelerations.ay.value(), 0.1); + EXPECT_NEAR(2 * std::numbers::pi, chassisAccelerations.alpha.value(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, + MixedTranslationRotationInverseAccelerations) { + ChassisAccelerations accelerations{2_mps_sq, 3_mps_sq, 1_rad_per_s_sq}; + auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations); + + EXPECT_NEAR(-25.0, wheelAccelerations.frontLeft.value(), 0.1); + EXPECT_NEAR(29.0, wheelAccelerations.frontRight.value(), 0.1); + EXPECT_NEAR(-19.0, wheelAccelerations.rearLeft.value(), 0.1); + EXPECT_NEAR(23.0, wheelAccelerations.rearRight.value(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, + MixedTranslationRotationForwardAccelerations) { + MecanumDriveWheelAccelerations wheelAccelerations{ + -17.677670_mps_sq, 20.51_mps_sq, -13.44_mps_sq, 16.26_mps_sq}; + auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); + + EXPECT_NEAR(1.413, chassisAccelerations.ax.value(), 0.1); + EXPECT_NEAR(2.122, chassisAccelerations.ay.value(), 0.1); + EXPECT_NEAR(0.707, chassisAccelerations.alpha.value(), 0.1); +} + +TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseAccelerations) { + ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, 1_rad_per_s_sq}; + auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations, m_fl); + + EXPECT_NEAR(0, wheelAccelerations.frontLeft.value(), 0.1); + EXPECT_NEAR(24.0, wheelAccelerations.frontRight.value(), 0.1); + EXPECT_NEAR(-24.0, wheelAccelerations.rearLeft.value(), 0.1); + EXPECT_NEAR(48.0, wheelAccelerations.rearRight.value(), 0.1); +} + diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index a851e562c22..947bd38d067 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -7,7 +7,9 @@ #include #include "frc/geometry/Translation2d.h" +#include "frc/kinematics/ChassisAccelerations.h" #include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/kinematics/SwerveModuleAccelerations.h" #include "units/angular_velocity.h" using namespace frc; @@ -309,3 +311,175 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) { EXPECT_NEAR(arr[2].speed.value(), -1.0, kEpsilon); EXPECT_NEAR(arr[3].speed.value(), -1.0, kEpsilon); } + +TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) { + ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, + units::radians_per_second_squared_t{ + 2 * std::numbers::pi}}; + units::radians_per_second_t angularVelocity = 2_rad_per_s * std::numbers::pi; + auto [flAccel, frAccel, blAccel, brAccel] = + m_kinematics.ToSwerveModuleAccelerations(accelerations, angularVelocity); + + // For turn-in-place with angular acceleration of 2π rad/s² and angular + // velocity of 2π rad/s, each module has both tangential acceleration (from + // angular acceleration) and centripetal acceleration (from angular velocity). + // The total acceleration magnitude is approximately 678.4. + // + // For each swerve module at position (x, y) relative to the robot center: + // - Distance from center: r = √(x² + y²) = √(12² + 12²) = 16.97 m + // - Current tangential velocity: v_t = ω × r = 2π × 16.97 = 106.63 m/s + // + // Two acceleration components: + // 1) Tangential acceleration (from angular acceleration α = 2π rad/s²): + // a_tangential = α × r = 2π × 16.97 = 106.63 m/s² + // Direction: perpendicular to radius vector + // + // 2) Centripetal acceleration (from angular velocity ω = 2π rad/s): + // a_centripetal = ω² × r = (2π)² × 16.97 = 668.7 m/s² + // Direction: toward center of rotation + // + // Total acceleration magnitude: |a_total| = √(a_tangential² + a_centripetal²) + // = √(106.63² + 668.7²) = 678.4 m/s² + // + // For module positions: + // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = + // -135° → total angle = -144° FR (12, -12): radius angle = 45°, tangential = + // -45°, centripetal = -135° → total angle = 126° BL (-12, 12): radius angle + // = 135°, tangential = 45°, centripetal = 45° → total angle = -54° BR + // (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → + // total angle = 36° + // + // The acceleration angles are not simply tangential because the centripetal + // component from the existing angular velocity dominates and affects the + // direction. + + double centerRadius = m_kinematics.GetModules()[0].Norm().value(); + double tangentialAccel = + centerRadius * accelerations.alpha.value(); // α * r + double centripetalAccel = centerRadius * angularVelocity.value() * angularVelocity.value(); // ω² * r + double totalAccel = std::hypot(tangentialAccel, centripetalAccel); + + std::array expectedAngles; + for (size_t i = 0; i < 4; i++) { + Rotation2d radiusAngle = m_kinematics.GetModules()[i].Angle(); + + // Tangential acceleration: perpendicular to radius (90° CCW from radius) + Rotation2d tangentialDirection = radiusAngle + Rotation2d{90_deg}; + double tangentialX = tangentialAccel * tangentialDirection.Cos(); + double tangentialY = tangentialAccel * tangentialDirection.Sin(); + + // Centripetal acceleration: toward center (opposite of radius) + Rotation2d centripetalDirection = radiusAngle + Rotation2d{180_deg}; + double centripetalX = centripetalAccel * centripetalDirection.Cos(); + double centripetalY = centripetalAccel * centripetalDirection.Sin(); + + // Vector sum of tangential and centripetal accelerations + double totalX = tangentialX + centripetalX; + double totalY = tangentialY + centripetalY; + + expectedAngles[i] = Rotation2d{totalX, totalY}; + } + + EXPECT_NEAR(totalAccel, flAccel.acceleration.value(), 0.1); + EXPECT_NEAR(totalAccel, frAccel.acceleration.value(), 0.1); + EXPECT_NEAR(totalAccel, blAccel.acceleration.value(), 0.1); + EXPECT_NEAR(totalAccel, brAccel.acceleration.value(), 0.1); + EXPECT_NEAR(expectedAngles[0].Degrees().value(), + flAccel.angle.Degrees().value(), 0.1); + EXPECT_NEAR(expectedAngles[1].Degrees().value(), + frAccel.angle.Degrees().value(), 0.1); + EXPECT_NEAR(expectedAngles[2].Degrees().value(), + blAccel.angle.Degrees().value(), 0.1); + EXPECT_NEAR(expectedAngles[3].Degrees().value(), + brAccel.angle.Degrees().value(), 0.1); +} + +TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardAccelerations) { + SwerveModuleAccelerations flAccel{106.629_mps_sq, 135_deg}; + SwerveModuleAccelerations frAccel{106.629_mps_sq, 45_deg}; + SwerveModuleAccelerations blAccel{106.629_mps_sq, -135_deg}; + SwerveModuleAccelerations brAccel{106.629_mps_sq, -45_deg}; + + auto chassisAccelerations = + m_kinematics.ToChassisAccelerations(flAccel, frAccel, blAccel, brAccel); + + EXPECT_NEAR(0.0, chassisAccelerations.ax.value(), 0.1); + EXPECT_NEAR(0.0, chassisAccelerations.ay.value(), 0.1); + EXPECT_NEAR(2 * std::numbers::pi, chassisAccelerations.alpha.value(), 0.1); +} + +TEST_F(SwerveDriveKinematicsTest, OffCenterRotationInverseAccelerations) { + ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, 1_rad_per_s_sq}; + // For this test, assume an angular velocity of 1 rad/s + units::radians_per_second_t angularVelocity = 1.0_rad_per_s; + auto [flAccel, frAccel, blAccel, brAccel] = + m_kinematics.ToSwerveModuleAccelerations(accelerations, angularVelocity, + m_fl); + + // When rotating about the front-left module position with both angular + // acceleration (1 rad/s²) and angular velocity (1 rad/s), each module + // experiences both tangential and centripetal accelerations that combine + // vectorially. + // + // Center of rotation: FL module at (12, 12) inches + // Module positions relative to center of rotation: + // - FL: (0, 0) - at center of rotation + // - FR: (0, -24) - 24 m south of center + // - BL: (-24, 0) - 24 m west of center + // - BR: (-24, -24) - distance = √(24² + 24²) = 33.94 m southwest + // + // For each module at distance r from center of rotation: + // 1) Tangential acceleration: a_t = α × r = 1 × r + // Direction: perpendicular to radius vector (90° CCW from radius) + // + // 2) Centripetal acceleration: a_c = ω² × r = 1² × r = r + // Direction: toward center of rotation + + std::array expectedAccelerations; + std::array expectedAngles; + + for (size_t i = 0; i < 4; i++) { + Translation2d relativePos = m_kinematics.GetModules()[i] - m_fl; + double r = relativePos.Norm().value(); + + if (r < 1e-9) { + expectedAccelerations[i] = 0.0; // No acceleration at center of rotation + expectedAngles[i] = Rotation2d{}; // Angle is undefined at center of rotation + } else { + double tangentialAccel = r * accelerations.alpha.value(); // α * r = 1 * r + double centripetalAccel = r * angularVelocity.value() * angularVelocity.value(); // ω² * r = 1 * r + expectedAccelerations[i] = std::hypot(tangentialAccel, centripetalAccel); + + Rotation2d radiusAngle{(relativePos.X().value()), (relativePos.Y().value())}; + + // Tangential acceleration: perpendicular to radius (90° CCW from radius) + Rotation2d tangentialDirection = radiusAngle + Rotation2d{90_deg}; + double tangentialX = tangentialDirection.Cos() * r; // α * r = 1 * r + double tangentialY = tangentialDirection.Sin() * r; + + // Centripetal acceleration: toward center (opposite of radius) + Rotation2d centripetalDirection = radiusAngle + Rotation2d{180_deg}; + double centripetalX = centripetalDirection.Cos() * r; // ω² * r = 1 * r + double centripetalY = centripetalDirection.Sin() * r; + + // Vector sum of tangential and centripetal accelerations + double totalX = tangentialX + centripetalX; + double totalY = tangentialY + centripetalY; + + expectedAngles[i] = Rotation2d{totalX, totalY}; + } + } + + EXPECT_NEAR(expectedAccelerations[0], flAccel.acceleration.value(), 0.1); + EXPECT_NEAR(expectedAccelerations[1], frAccel.acceleration.value(), 0.1); + EXPECT_NEAR(expectedAccelerations[2], blAccel.acceleration.value(), 0.1); + EXPECT_NEAR(expectedAccelerations[3], brAccel.acceleration.value(), 0.1); + EXPECT_NEAR(expectedAngles[0].Degrees().value(), + flAccel.angle.Degrees().value(), 0.1); + EXPECT_NEAR(expectedAngles[1].Degrees().value(), + frAccel.angle.Degrees().value(), 0.1); + EXPECT_NEAR(expectedAngles[2].Degrees().value(), + blAccel.angle.Degrees().value(), 0.1); + EXPECT_NEAR(expectedAngles[3].Degrees().value(), + brAccel.angle.Degrees().value(), 0.1); +} From 1dad79e10914c3348fd5ece873405e5a31c94a0b Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 14:58:08 -0400 Subject: [PATCH 49/82] make wpiformat happy one last time Signed-off-by: Zach Harel --- .../frc/kinematics/MecanumDriveKinematics.h | 2 +- .../frc/kinematics/SwerveDriveKinematics.h | 6 +++-- .../DifferentialDriveKinematicsTest.cpp | 4 +-- .../kinematics/MecanumDriveKinematicsTest.cpp | 24 ++++++++++------- .../kinematics/SwerveDriveKinematicsTest.cpp | 26 +++++++++++-------- 5 files changed, 36 insertions(+), 26 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index d6aa0c344e1..21f38d7466d 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -174,7 +174,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics } ChassisAccelerations ToChassisAccelerations( - const MecanumDriveWheelAccelerations& wheelAccelerations) const override; + const MecanumDriveWheelAccelerations& wheelAccelerations) const override; MecanumDriveWheelAccelerations ToWheelAccelerations( const ChassisAccelerations& chassisAccelerations, diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index f9cc040c266..e2bf3966387 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -542,12 +542,14 @@ class SwerveDriveKinematics * accelerations should be same as passed into the constructor of this class. * @return The resulting chassis accelerations. */ - template ... ModuleAccelerations> + template < + std::convertible_to... ModuleAccelerations> requires(sizeof...(ModuleAccelerations) == NumModules) ChassisAccelerations ToChassisAccelerations( ModuleAccelerations&&... moduleAccelerations) const { return this->ToChassisAccelerations( - wpi::array{moduleAccelerations...}); + wpi::array{ + moduleAccelerations...}); } /** diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp index 510d77b69d1..6664d9848c4 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp @@ -102,7 +102,7 @@ TEST(DifferentialDriveKinematicsTest, ForwardAccelerationsForZeros) { TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForStraightLine) { const DifferentialDriveKinematics kinematics{0.381_m * 2}; const ChassisAccelerations chassisAccelerations{3.0_mps_sq, 0_mps_sq, - 0_rad_per_s_sq}; + 0_rad_per_s_sq}; const auto wheelAccelerations = kinematics.ToWheelAccelerations(chassisAccelerations); @@ -113,7 +113,7 @@ TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForStraightLine) { TEST(DifferentialDriveKinematicsTest, ForwardAccelerationsForStraightLine) { const DifferentialDriveKinematics kinematics{0.381_m * 2}; const DifferentialDriveWheelAccelerations wheelAccelerations{3.0_mps_sq, - 3.0_mps_sq}; + 3.0_mps_sq}; const auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index ffa87692c0e..5895321c0f0 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -242,8 +242,9 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineInverseAccelerations) { TEST_F(MecanumDriveKinematicsTest, StraightLineForwardAccelerations) { MecanumDriveWheelAccelerations wheelAccelerations{3.536_mps_sq, 3.536_mps_sq, - 3.536_mps_sq, 3.536_mps_sq}; - auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); + 3.536_mps_sq, 3.536_mps_sq}; + auto chassisAccelerations = + kinematics.ToChassisAccelerations(wheelAccelerations); EXPECT_NEAR(3.536, chassisAccelerations.ax.value(), 0.1); EXPECT_NEAR(0, chassisAccelerations.ay.value(), 0.1); @@ -263,7 +264,8 @@ TEST_F(MecanumDriveKinematicsTest, StrafeInverseAccelerations) { TEST_F(MecanumDriveKinematicsTest, StrafeForwardAccelerations) { MecanumDriveWheelAccelerations wheelAccelerations{ -2.828427_mps_sq, 2.828427_mps_sq, 2.828427_mps_sq, -2.828427_mps_sq}; - auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); + auto chassisAccelerations = + kinematics.ToChassisAccelerations(wheelAccelerations); EXPECT_NEAR(0, chassisAccelerations.ax.value(), 0.1); EXPECT_NEAR(2.8284, chassisAccelerations.ay.value(), 0.1); @@ -271,9 +273,9 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardAccelerations) { } TEST_F(MecanumDriveKinematicsTest, RotationInverseAccelerations) { - ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, - units::radians_per_second_squared_t{ - 2 * std::numbers::pi}}; + ChassisAccelerations accelerations{ + 0_mps_sq, 0_mps_sq, + units::radians_per_second_squared_t{2 * std::numbers::pi}}; auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations); EXPECT_NEAR(-150.79645, wheelAccelerations.frontLeft.value(), 0.1); @@ -285,7 +287,8 @@ TEST_F(MecanumDriveKinematicsTest, RotationInverseAccelerations) { TEST_F(MecanumDriveKinematicsTest, RotationForwardAccelerations) { MecanumDriveWheelAccelerations wheelAccelerations{ -150.79645_mps_sq, 150.79645_mps_sq, -150.79645_mps_sq, 150.79645_mps_sq}; - auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); + auto chassisAccelerations = + kinematics.ToChassisAccelerations(wheelAccelerations); EXPECT_NEAR(0, chassisAccelerations.ax.value(), 0.1); EXPECT_NEAR(0, chassisAccelerations.ay.value(), 0.1); @@ -307,7 +310,8 @@ TEST_F(MecanumDriveKinematicsTest, MixedTranslationRotationForwardAccelerations) { MecanumDriveWheelAccelerations wheelAccelerations{ -17.677670_mps_sq, 20.51_mps_sq, -13.44_mps_sq, 16.26_mps_sq}; - auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); + auto chassisAccelerations = + kinematics.ToChassisAccelerations(wheelAccelerations); EXPECT_NEAR(1.413, chassisAccelerations.ax.value(), 0.1); EXPECT_NEAR(2.122, chassisAccelerations.ay.value(), 0.1); @@ -316,11 +320,11 @@ TEST_F(MecanumDriveKinematicsTest, TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseAccelerations) { ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, 1_rad_per_s_sq}; - auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations, m_fl); + auto wheelAccelerations = + kinematics.ToWheelAccelerations(accelerations, m_fl); EXPECT_NEAR(0, wheelAccelerations.frontLeft.value(), 0.1); EXPECT_NEAR(24.0, wheelAccelerations.frontRight.value(), 0.1); EXPECT_NEAR(-24.0, wheelAccelerations.rearLeft.value(), 0.1); EXPECT_NEAR(48.0, wheelAccelerations.rearRight.value(), 0.1); } - diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index 947bd38d067..935110fe20e 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -313,9 +313,9 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) { } TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) { - ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, - units::radians_per_second_squared_t{ - 2 * std::numbers::pi}}; + ChassisAccelerations accelerations{ + 0_mps_sq, 0_mps_sq, + units::radians_per_second_squared_t{2 * std::numbers::pi}}; units::radians_per_second_t angularVelocity = 2_rad_per_s * std::numbers::pi; auto [flAccel, frAccel, blAccel, brAccel] = m_kinematics.ToSwerveModuleAccelerations(accelerations, angularVelocity); @@ -354,9 +354,9 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) { // direction. double centerRadius = m_kinematics.GetModules()[0].Norm().value(); - double tangentialAccel = - centerRadius * accelerations.alpha.value(); // α * r - double centripetalAccel = centerRadius * angularVelocity.value() * angularVelocity.value(); // ω² * r + double tangentialAccel = centerRadius * accelerations.alpha.value(); // α * r + double centripetalAccel = centerRadius * angularVelocity.value() * + angularVelocity.value(); // ω² * r double totalAccel = std::hypot(tangentialAccel, centripetalAccel); std::array expectedAngles; @@ -414,7 +414,7 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterRotationInverseAccelerations) { units::radians_per_second_t angularVelocity = 1.0_rad_per_s; auto [flAccel, frAccel, blAccel, brAccel] = m_kinematics.ToSwerveModuleAccelerations(accelerations, angularVelocity, - m_fl); + m_fl); // When rotating about the front-left module position with both angular // acceleration (1 rad/s²) and angular velocity (1 rad/s), each module @@ -444,13 +444,17 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterRotationInverseAccelerations) { if (r < 1e-9) { expectedAccelerations[i] = 0.0; // No acceleration at center of rotation - expectedAngles[i] = Rotation2d{}; // Angle is undefined at center of rotation + expectedAngles[i] = + Rotation2d{}; // Angle is undefined at center of rotation } else { - double tangentialAccel = r * accelerations.alpha.value(); // α * r = 1 * r - double centripetalAccel = r * angularVelocity.value() * angularVelocity.value(); // ω² * r = 1 * r + double tangentialAccel = + r * accelerations.alpha.value(); // α * r = 1 * r + double centripetalAccel = r * angularVelocity.value() * + angularVelocity.value(); // ω² * r = 1 * r expectedAccelerations[i] = std::hypot(tangentialAccel, centripetalAccel); - Rotation2d radiusAngle{(relativePos.X().value()), (relativePos.Y().value())}; + Rotation2d radiusAngle{(relativePos.X().value()), + (relativePos.Y().value())}; // Tangential acceleration: perpendicular to radius (90° CCW from radius) Rotation2d tangentialDirection = radiusAngle + Rotation2d{90_deg}; From 32bed43cd96114c7ed8729db2329b259562c95e1 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 16:21:23 -0400 Subject: [PATCH 50/82] refactor: improve code formatting for clarity in MecanumDriveKinematics methods Signed-off-by: Zach Harel --- .../cpp/kinematics/MecanumDriveKinematics.cpp | 53 +++++++++++-------- 1 file changed, 30 insertions(+), 23 deletions(-) diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index 970bb9e718c..df410059888 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -88,17 +88,19 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl, ChassisAccelerations MecanumDriveKinematics::ToChassisAccelerations( const MecanumDriveWheelAccelerations& wheelAccelerations) const { - Eigen::Vector4d wheelSpeedsVector{wheelAccelerations.frontLeft.value(), - wheelAccelerations.frontRight.value(), - wheelAccelerations.rearLeft.value(), - wheelAccelerations.rearRight.value()}; - - Eigen::Vector3d chassisSpeedsVector = - m_forwardKinematics.solve(wheelSpeedsVector); - - return {units::meters_per_second_squared_t{chassisSpeedsVector(0)}, // NOLINT - units::meters_per_second_squared_t{chassisSpeedsVector(1)}, - units::radians_per_second_squared_t{chassisSpeedsVector(2)}}; + Eigen::Vector4d wheelAccelerationsVector{ + wheelAccelerations.frontLeft.value(), + wheelAccelerations.frontRight.value(), + wheelAccelerations.rearLeft.value(), + wheelAccelerations.rearRight.value()}; + + Eigen::Vector3d chassisAccelerationsVector = + m_forwardKinematics.solve(wheelAccelerationsVector); + + return {units::meters_per_second_squared_t{ + chassisAccelerationsVector(0)}, // NOLINT + units::meters_per_second_squared_t{chassisAccelerationsVector(1)}, + units::radians_per_second_squared_t{chassisAccelerationsVector(2)}}; } MecanumDriveWheelAccelerations MecanumDriveKinematics::ToWheelAccelerations( @@ -116,16 +118,21 @@ MecanumDriveWheelAccelerations MecanumDriveKinematics::ToWheelAccelerations( m_previousCoR = centerOfRotation; } - Eigen::Vector3d chassisSpeedsVector{chassisAccelerations.ax.value(), - chassisAccelerations.ay.value(), - chassisAccelerations.alpha.value()}; - - Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector; - - MecanumDriveWheelAccelerations wheelAccels; - wheelAccels.frontLeft = units::meters_per_second_squared_t{wheelsVector(0)}; - wheelAccels.frontRight = units::meters_per_second_squared_t{wheelsVector(1)}; - wheelAccels.rearLeft = units::meters_per_second_squared_t{wheelsVector(2)}; - wheelAccels.rearRight = units::meters_per_second_squared_t{wheelsVector(3)}; - return wheelAccels; + Eigen::Vector3d chassisAccelerationsVector{ + chassisAccelerations.ax.value(), chassisAccelerations.ay.value(), + chassisAccelerations.alpha.value()}; + + Eigen::Vector4d wheelsVector = + m_inverseKinematics * chassisAccelerationsVector; + + MecanumDriveWheelAccelerations wheelAccelerations; + wheelAccelerations.frontLeft = + units::meters_per_second_squared_t{wheelsVector(0)}; + wheelAccelerations.frontRight = + units::meters_per_second_squared_t{wheelsVector(1)}; + wheelAccelerations.rearLeft = + units::meters_per_second_squared_t{wheelsVector(2)}; + wheelAccelerations.rearRight = + units::meters_per_second_squared_t{wheelsVector(3)}; + return wheelAccelerations; } From 23c3dccaafc2f908d5cd9bd4d779870cf40adde6 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 17:00:34 -0400 Subject: [PATCH 51/82] fix: remove NOLINT from MecanumDriveKinematics.cpp Signed-off-by: Zach Harel --- .../main/native/cpp/kinematics/MecanumDriveKinematics.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index df410059888..2e7cc7f1ec0 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -44,7 +44,7 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds( Eigen::Vector3d chassisSpeedsVector = m_forwardKinematics.solve(wheelSpeedsVector); - return {units::meters_per_second_t{chassisSpeedsVector(0)}, // NOLINT + return {units::meters_per_second_t{chassisSpeedsVector(0)}, units::meters_per_second_t{chassisSpeedsVector(1)}, units::radians_per_second_t{chassisSpeedsVector(2)}}; } @@ -97,8 +97,7 @@ ChassisAccelerations MecanumDriveKinematics::ToChassisAccelerations( Eigen::Vector3d chassisAccelerationsVector = m_forwardKinematics.solve(wheelAccelerationsVector); - return {units::meters_per_second_squared_t{ - chassisAccelerationsVector(0)}, // NOLINT + return {units::meters_per_second_squared_t{chassisAccelerationsVector(0)}, units::meters_per_second_squared_t{chassisAccelerationsVector(1)}, units::radians_per_second_squared_t{chassisAccelerationsVector(2)}}; } From 1a8faf46b39d4df2cfd6fa68a3a2d92a3da22123 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 20:11:47 -0400 Subject: [PATCH 52/82] refactor: remove wrong comments in SwerveDriveKinematics.java Signed-off-by: Zach Harel --- .../edu/wpi/first/math/kinematics/SwerveDriveKinematics.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index e55e318a26c..73c58bdc444 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -448,9 +448,6 @@ public static final SwerveDriveKinematicsStruct getStruct(int numModules) { * argument is defaulted to that use case. However, if you wish to change the center of rotation * for evasive maneuvers, vision alignment, or for any other use case, you can do so. * - *

In the case that the desired chassis accelerations are zero (i.e. the robot will be - * stationary), the previously calculated module angle will be maintained. - * * @param chassisAccelerations The desired chassis accelerations. * @param angularVelocity The desired robot angular velocity. * @param centerOfRotation The center of rotation. For example, if you set the center of rotation From 6b07fba361efa30964d4fc466d13969bc31a4991 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 11 Oct 2025 20:11:54 -0400 Subject: [PATCH 53/82] refactor: correct Rotation2d initialization and simplify ToWheelAccelerations method Signed-off-by: Zach Harel --- .../native/include/frc/kinematics/SwerveDriveKinematics.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index e2bf3966387..6bd16580c03 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -480,8 +480,7 @@ class SwerveDriveKinematics chassisAccelerations.ay == 0.0_mps_sq && chassisAccelerations.alpha == 0.0_rad_per_s_sq) { for (size_t i = 0; i < NumModules; i++) { - moduleAccelerations[i] = { - 0.0_mps_sq, Rotation2d{0.0, 0.0}}; // maintain previous angle + moduleAccelerations[i] = {0.0_mps_sq, Rotation2d{0.0_rad}}; } return moduleAccelerations; } @@ -510,7 +509,7 @@ class SwerveDriveKinematics units::math::hypot(x, y); moduleAccelerations[i] = {linearAcceleration, - Rotation2d{(x.value()), y.value()}}; + Rotation2d{x.value(), y.value()}}; } return moduleAccelerations; @@ -527,8 +526,7 @@ class SwerveDriveKinematics */ wpi::array ToWheelAccelerations( const ChassisAccelerations& chassisAccelerations) const override { - return ToSwerveModuleAccelerations(chassisAccelerations, 0.0_rad_per_s, - Translation2d{}); + return ToSwerveModuleAccelerations(chassisAccelerations); } /** From 4afef0b6707e692d1260e23a2c9f3cd588287977 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sun, 12 Oct 2025 20:43:17 -0400 Subject: [PATCH 54/82] docs: add reference to second-order kinematics derivation in SwerveDriveKinematics Signed-off-by: Zach Harel --- .../wpi/first/math/kinematics/SwerveDriveKinematics.java | 6 ++++++ .../native/include/frc/kinematics/SwerveDriveKinematics.h | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index 73c58bdc444..cfcf459f780 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -448,6 +448,9 @@ public static final SwerveDriveKinematicsStruct getStruct(int numModules) { * argument is defaulted to that use case. However, if you wish to change the center of rotation * for evasive maneuvers, vision alignment, or for any other use case, you can do so. * + *

A derivation for the second-order kinematics can be found + * here. + * * @param chassisAccelerations The desired chassis accelerations. * @param angularVelocity The desired robot angular velocity. * @param centerOfRotation The center of rotation. For example, if you set the center of rotation @@ -525,6 +528,9 @@ public SwerveModuleAccelerations[] toWheelAccelerations( * acceleration on the field using data from the real-world acceleration of each module on the * robot. * + *

A derivation for the second-order kinematics can be found + * here. + * * @param moduleAccelerations The accelerations of the modules as measured from respective * encoders and gyros. The order of the swerve module accelerations should be same as passed * into the constructor of this class. diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 6bd16580c03..8f915ff08ee 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -461,6 +461,9 @@ class SwerveDriveKinematics * However, if you wish to change the center of rotation for evasive * maneuvers, vision alignment, or for any other use case, you can do so. * + *

A derivation for the second-order kinematics can be found + * here. + * * @param chassisAccelerations The desired chassis accelerations. * @param angularVelocity The desired robot angular velocity. * @param centerOfRotation The center of rotation. For example, if you set the @@ -556,6 +559,9 @@ class SwerveDriveKinematics * calculations -- determining the robot's acceleration on the field using * data from the real-world acceleration of each module on the robot. * + *

A derivation for the second-order kinematics can be found + * here. + * * @param moduleAccelerations The accelerations of the modules as measured * from respective encoders and gyros. The order of the swerve module * accelerations should be same as passed into the constructor of this class. From 77f9bb022fed02b123d9459a2dabfb3f7682169a Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Wed, 15 Oct 2025 10:08:13 -0400 Subject: [PATCH 55/82] docs: cite whitepaper for swerve kinematics with a comment instead of javadoc Signed-off-by: Zach Harel --- .../math/kinematics/SwerveDriveKinematics.java | 14 ++++++++------ .../include/frc/kinematics/SwerveDriveKinematics.h | 14 ++++++++------ 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index cfcf459f780..f5c230e89b7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -448,9 +448,6 @@ public static final SwerveDriveKinematicsStruct getStruct(int numModules) { * argument is defaulted to that use case. However, if you wish to change the center of rotation * for evasive maneuvers, vision alignment, or for any other use case, you can do so. * - *

A derivation for the second-order kinematics can be found - * here. - * * @param chassisAccelerations The desired chassis accelerations. * @param angularVelocity The desired robot angular velocity. * @param centerOfRotation The center of rotation. For example, if you set the center of rotation @@ -462,6 +459,10 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( ChassisAccelerations chassisAccelerations, double angularVelocity, Translation2d centerOfRotation) { + // Derivation for second-order kinematics from "Swerve Drive Second Order Kinematics" + // by FRC Team 449 - The Blair Robot Project, Rafi Pedersen + // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf + var moduleAccelerations = new SwerveModuleAccelerations[m_numModules]; if (chassisAccelerations.ax == 0.0 @@ -528,9 +529,6 @@ public SwerveModuleAccelerations[] toWheelAccelerations( * acceleration on the field using data from the real-world acceleration of each module on the * robot. * - *

A derivation for the second-order kinematics can be found - * here. - * * @param moduleAccelerations The accelerations of the modules as measured from respective * encoders and gyros. The order of the swerve module accelerations should be same as passed * into the constructor of this class. @@ -539,6 +537,10 @@ public SwerveModuleAccelerations[] toWheelAccelerations( @Override public ChassisAccelerations toChassisAccelerations( SwerveModuleAccelerations... moduleAccelerations) { + // Derivation for second-order kinematics from "Swerve Drive Second Order Kinematics" + // by FRC Team 449 - The Blair Robot Project, Rafi Pedersen + // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf + if (moduleAccelerations.length != m_numModules) { throw new IllegalArgumentException( "Number of modules is not consistent with number of module locations provided in " diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 8f915ff08ee..f04037111eb 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -461,9 +461,6 @@ class SwerveDriveKinematics * However, if you wish to change the center of rotation for evasive * maneuvers, vision alignment, or for any other use case, you can do so. * - *

A derivation for the second-order kinematics can be found - * here. - * * @param chassisAccelerations The desired chassis accelerations. * @param angularVelocity The desired robot angular velocity. * @param centerOfRotation The center of rotation. For example, if you set the @@ -476,6 +473,10 @@ class SwerveDriveKinematics const ChassisAccelerations& chassisAccelerations, const units::radians_per_second_t angularVelocity = 0.0_rad_per_s, const Translation2d& centerOfRotation = Translation2d{}) const { + // Derivation for second-order kinematics from "Swerve Drive Second Order Kinematics" + // by FRC Team 449 - The Blair Robot Project, Rafi Pedersen + // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf + wpi::array moduleAccelerations( wpi::empty_array); @@ -559,9 +560,6 @@ class SwerveDriveKinematics * calculations -- determining the robot's acceleration on the field using * data from the real-world acceleration of each module on the robot. * - *

A derivation for the second-order kinematics can be found - * here. - * * @param moduleAccelerations The accelerations of the modules as measured * from respective encoders and gyros. The order of the swerve module * accelerations should be same as passed into the constructor of this class. @@ -570,6 +568,10 @@ class SwerveDriveKinematics ChassisAccelerations ToChassisAccelerations( const wpi::array& moduleAccelerations) const override { + // Derivation for second-order kinematics from "Swerve Drive Second Order Kinematics" + // by FRC Team 449 - The Blair Robot Project, Rafi Pedersen + // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf + Matrixd moduleAccelerationsMatrix; for (size_t i = 0; i < NumModules; i++) { From 8e58cd497178066785b36aa2968b275611180ff6 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Wed, 15 Oct 2025 16:33:07 -0400 Subject: [PATCH 56/82] make wpiformat happy Signed-off-by: Zach Harel --- .../native/include/frc/kinematics/SwerveDriveKinematics.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index f04037111eb..705a1bf12ab 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -473,8 +473,8 @@ class SwerveDriveKinematics const ChassisAccelerations& chassisAccelerations, const units::radians_per_second_t angularVelocity = 0.0_rad_per_s, const Translation2d& centerOfRotation = Translation2d{}) const { - // Derivation for second-order kinematics from "Swerve Drive Second Order Kinematics" - // by FRC Team 449 - The Blair Robot Project, Rafi Pedersen + // Derivation for second-order kinematics from "Swerve Drive Second Order + // Kinematics" by FRC Team 449 - The Blair Robot Project, Rafi Pedersen // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf wpi::array moduleAccelerations( @@ -568,8 +568,8 @@ class SwerveDriveKinematics ChassisAccelerations ToChassisAccelerations( const wpi::array& moduleAccelerations) const override { - // Derivation for second-order kinematics from "Swerve Drive Second Order Kinematics" - // by FRC Team 449 - The Blair Robot Project, Rafi Pedersen + // Derivation for second-order kinematics from "Swerve Drive Second Order + // Kinematics" by FRC Team 449 - The Blair Robot Project, Rafi Pedersen // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf Matrixd moduleAccelerationsMatrix; From 808d9e72d1d6a01e057ef7cdac808d8282aed977 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 17 Oct 2025 11:20:16 -0400 Subject: [PATCH 57/82] refactor: handle small linear acceleration case in SwerveModuleAccelerations Signed-off-by: Zach Harel --- .../wpi/first/math/kinematics/SwerveDriveKinematics.java | 9 +++++++-- .../include/frc/kinematics/SwerveDriveKinematics.h | 6 +++++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index f5c230e89b7..9d4417ff661 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -497,8 +497,13 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( // The linear acceleration is the magnitude of the acceleration vector double linearAcceleration = Math.hypot(x, y); - moduleAccelerations[i] = - new SwerveModuleAccelerations(linearAcceleration, new Rotation2d(x, y)); + if (linearAcceleration < 1e-6) { + moduleAccelerations[i] = + new SwerveModuleAccelerations(linearAcceleration, Rotation2d.kZero); + } else { + moduleAccelerations[i] = + new SwerveModuleAccelerations(linearAcceleration, new Rotation2d(x, y)); + } } return moduleAccelerations; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 705a1bf12ab..8f998e746fc 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -512,8 +512,12 @@ class SwerveDriveKinematics units::meters_per_second_squared_t linearAcceleration = units::math::hypot(x, y); - moduleAccelerations[i] = {linearAcceleration, + if (linearAcceleration.value() < 1e-6) { + moduleAccelerations[i] = {linearAcceleration, {}}; + } else { + moduleAccelerations[i] = {linearAcceleration, Rotation2d{x.value(), y.value()}}; + } } return moduleAccelerations; From e2baa93b801cbb7d9c92f2ee484e1e7a846871a1 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 17 Oct 2025 12:05:21 -0400 Subject: [PATCH 58/82] test: update assertions in SwerveDriveKinematicsTest to use kEpsilon for precision Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematicsTest.java | 36 +++++++++--------- .../kinematics/SwerveDriveKinematicsTest.cpp | 38 +++++++++---------- 2 files changed, 37 insertions(+), 37 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index dfed9478045..b5a5dd0b71a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -471,22 +471,22 @@ void testTurnInPlaceInverseAccelerations() { .toArray(Rotation2d[]::new); assertAll( - () -> assertEquals(totalAccel, moduleAccelerations[0].acceleration, 0.1), - () -> assertEquals(totalAccel, moduleAccelerations[1].acceleration, 0.1), - () -> assertEquals(totalAccel, moduleAccelerations[2].acceleration, 0.1), - () -> assertEquals(totalAccel, moduleAccelerations[3].acceleration, 0.1), + () -> assertEquals(totalAccel, moduleAccelerations[0].acceleration, kEpsilon), + () -> assertEquals(totalAccel, moduleAccelerations[1].acceleration, kEpsilon), + () -> assertEquals(totalAccel, moduleAccelerations[2].acceleration, kEpsilon), + () -> assertEquals(totalAccel, moduleAccelerations[3].acceleration, kEpsilon), () -> assertEquals( - expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), 0.1), + expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), kEpsilon), () -> assertEquals( - expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), 0.1), + expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), kEpsilon), () -> assertEquals( - expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), 0.1), + expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), kEpsilon), () -> assertEquals( - expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), 0.1)); + expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), kEpsilon)); } @Test @@ -504,8 +504,8 @@ void testTurnInPlaceForwardAccelerations() { m_kinematics.toChassisAccelerations(flAccel, frAccel, blAccel, brAccel); assertAll( - () -> assertEquals(0.0, chassisAccelerations.ax, 0.1), - () -> assertEquals(0.0, chassisAccelerations.ay, 0.1), + () -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon), + () -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon), () -> assertEquals(2 * Math.PI, chassisAccelerations.alpha, 0.1)); } @@ -584,21 +584,21 @@ void testOffCenterRotationInverseAccelerations() { .toArray(Rotation2d[]::new); assertAll( - () -> assertEquals(expectedAccelerations[0], moduleAccelerations[0].acceleration, 0.1), - () -> assertEquals(expectedAccelerations[1], moduleAccelerations[1].acceleration, 0.1), - () -> assertEquals(expectedAccelerations[2], moduleAccelerations[2].acceleration, 0.1), - () -> assertEquals(expectedAccelerations[3], moduleAccelerations[3].acceleration, 0.1), + () -> assertEquals(expectedAccelerations[0], moduleAccelerations[0].acceleration, kEpsilon), + () -> assertEquals(expectedAccelerations[1], moduleAccelerations[1].acceleration, kEpsilon), + () -> assertEquals(expectedAccelerations[2], moduleAccelerations[2].acceleration, kEpsilon), + () -> assertEquals(expectedAccelerations[3], moduleAccelerations[3].acceleration, kEpsilon), () -> assertEquals( - expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), 0.1), + expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), kEpsilon), () -> assertEquals( - expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), 0.1), + expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), kEpsilon), () -> assertEquals( - expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), 0.1), + expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), kEpsilon), () -> assertEquals( - expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), 0.1)); + expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), kEpsilon)); } } diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index 935110fe20e..7f472b145f1 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -380,18 +380,18 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) { expectedAngles[i] = Rotation2d{totalX, totalY}; } - EXPECT_NEAR(totalAccel, flAccel.acceleration.value(), 0.1); - EXPECT_NEAR(totalAccel, frAccel.acceleration.value(), 0.1); - EXPECT_NEAR(totalAccel, blAccel.acceleration.value(), 0.1); - EXPECT_NEAR(totalAccel, brAccel.acceleration.value(), 0.1); + EXPECT_NEAR(totalAccel, flAccel.acceleration.value(), kEpsilon); + EXPECT_NEAR(totalAccel, frAccel.acceleration.value(), kEpsilon); + EXPECT_NEAR(totalAccel, blAccel.acceleration.value(), kEpsilon); + EXPECT_NEAR(totalAccel, brAccel.acceleration.value(), kEpsilon); EXPECT_NEAR(expectedAngles[0].Degrees().value(), - flAccel.angle.Degrees().value(), 0.1); + flAccel.angle.Degrees().value(), kEpsilon); EXPECT_NEAR(expectedAngles[1].Degrees().value(), - frAccel.angle.Degrees().value(), 0.1); + frAccel.angle.Degrees().value(), kEpsilon); EXPECT_NEAR(expectedAngles[2].Degrees().value(), - blAccel.angle.Degrees().value(), 0.1); + blAccel.angle.Degrees().value(), kEpsilon); EXPECT_NEAR(expectedAngles[3].Degrees().value(), - brAccel.angle.Degrees().value(), 0.1); + brAccel.angle.Degrees().value(), kEpsilon); } TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardAccelerations) { @@ -403,9 +403,9 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardAccelerations) { auto chassisAccelerations = m_kinematics.ToChassisAccelerations(flAccel, frAccel, blAccel, brAccel); - EXPECT_NEAR(0.0, chassisAccelerations.ax.value(), 0.1); - EXPECT_NEAR(0.0, chassisAccelerations.ay.value(), 0.1); - EXPECT_NEAR(2 * std::numbers::pi, chassisAccelerations.alpha.value(), 0.1); + EXPECT_NEAR(0.0, chassisAccelerations.ax.value(), kEpsilon); + EXPECT_NEAR(0.0, chassisAccelerations.ay.value(), kEpsilon); + EXPECT_NEAR(2 * std::numbers::pi, chassisAccelerations.alpha.value(), kEpsilon); } TEST_F(SwerveDriveKinematicsTest, OffCenterRotationInverseAccelerations) { @@ -474,16 +474,16 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterRotationInverseAccelerations) { } } - EXPECT_NEAR(expectedAccelerations[0], flAccel.acceleration.value(), 0.1); - EXPECT_NEAR(expectedAccelerations[1], frAccel.acceleration.value(), 0.1); - EXPECT_NEAR(expectedAccelerations[2], blAccel.acceleration.value(), 0.1); - EXPECT_NEAR(expectedAccelerations[3], brAccel.acceleration.value(), 0.1); + EXPECT_NEAR(expectedAccelerations[0], flAccel.acceleration.value(), kEpsilon); + EXPECT_NEAR(expectedAccelerations[1], frAccel.acceleration.value(), kEpsilon); + EXPECT_NEAR(expectedAccelerations[2], blAccel.acceleration.value(), kEpsilon); + EXPECT_NEAR(expectedAccelerations[3], brAccel.acceleration.value(), kEpsilon); EXPECT_NEAR(expectedAngles[0].Degrees().value(), - flAccel.angle.Degrees().value(), 0.1); + flAccel.angle.Degrees().value(), kEpsilon); EXPECT_NEAR(expectedAngles[1].Degrees().value(), - frAccel.angle.Degrees().value(), 0.1); + frAccel.angle.Degrees().value(), kEpsilon); EXPECT_NEAR(expectedAngles[2].Degrees().value(), - blAccel.angle.Degrees().value(), 0.1); + blAccel.angle.Degrees().value(), kEpsilon); EXPECT_NEAR(expectedAngles[3].Degrees().value(), - brAccel.angle.Degrees().value(), 0.1); + brAccel.angle.Degrees().value(), kEpsilon); } From a77cbaf624392f266ef9b430578f5de6f8b89c57 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 18 Oct 2025 19:57:11 -0400 Subject: [PATCH 59/82] commit suggestion !? Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../edu/wpi/first/math/kinematics/SwerveDriveKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index 9d4417ff661..c8cef614636 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -497,7 +497,7 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( // The linear acceleration is the magnitude of the acceleration vector double linearAcceleration = Math.hypot(x, y); - if (linearAcceleration < 1e-6) { + if (linearAcceleration <= 1e-6) { moduleAccelerations[i] = new SwerveModuleAccelerations(linearAcceleration, Rotation2d.kZero); } else { From 5d2a28b3fe5fb7ea0fcd0673ed34e2e27ac4bfed Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 30 Oct 2025 09:58:08 -0400 Subject: [PATCH 60/82] test: update comparison assertion in SwerveModuleAccelerationsTest for accuracy Signed-off-by: Zach Harel --- .../first/math/kinematics/SwerveModuleAccelerationsTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java index 3db682cc925..adf92942caa 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleAccelerationsTest.java @@ -62,7 +62,7 @@ void testCompareTo() { assertTrue(slower.compareTo(faster) < 0); assertTrue(faster.compareTo(slower) > 0); - assertEquals(0, slower.compareTo(slower)); + assertEquals(0, slower.compareTo(new SwerveModuleAccelerations(1.0, new Rotation2d(2.0)))); } @Test From 4b0be2dd4082be52bbefbd2f94cb9b44b2b9df1a Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 30 Oct 2025 09:58:47 -0400 Subject: [PATCH 61/82] style: improve code formatting in SwerveDriveKinematics and SwerveDriveKinematicsTest Signed-off-by: Zach Harel --- .../main/native/include/frc/kinematics/SwerveDriveKinematics.h | 2 +- .../test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 8f998e746fc..fab37c26a2a 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -516,7 +516,7 @@ class SwerveDriveKinematics moduleAccelerations[i] = {linearAcceleration, {}}; } else { moduleAccelerations[i] = {linearAcceleration, - Rotation2d{x.value(), y.value()}}; + Rotation2d{x.value(), y.value()}}; } } diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index 7f472b145f1..6df80324fb1 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -405,7 +405,8 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardAccelerations) { EXPECT_NEAR(0.0, chassisAccelerations.ax.value(), kEpsilon); EXPECT_NEAR(0.0, chassisAccelerations.ay.value(), kEpsilon); - EXPECT_NEAR(2 * std::numbers::pi, chassisAccelerations.alpha.value(), kEpsilon); + EXPECT_NEAR(2 * std::numbers::pi, chassisAccelerations.alpha.value(), + kEpsilon); } TEST_F(SwerveDriveKinematicsTest, OffCenterRotationInverseAccelerations) { From 6cada918c02f81d63db22c640af2d2caddb9bc74 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 30 Oct 2025 09:59:30 -0400 Subject: [PATCH 62/82] test: improve readability of assertions in SwerveDriveKinematicsTest Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematicsTest.java | 32 ++++++++++++++----- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index b5a5dd0b71a..856b0958e30 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -477,16 +477,24 @@ void testTurnInPlaceInverseAccelerations() { () -> assertEquals(totalAccel, moduleAccelerations[3].acceleration, kEpsilon), () -> assertEquals( - expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), kEpsilon), + expectedAngles[0].getDegrees(), + moduleAccelerations[0].angle.getDegrees(), + kEpsilon), () -> assertEquals( - expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), kEpsilon), + expectedAngles[1].getDegrees(), + moduleAccelerations[1].angle.getDegrees(), + kEpsilon), () -> assertEquals( - expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), kEpsilon), + expectedAngles[2].getDegrees(), + moduleAccelerations[2].angle.getDegrees(), + kEpsilon), () -> assertEquals( - expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), kEpsilon)); + expectedAngles[3].getDegrees(), + moduleAccelerations[3].angle.getDegrees(), + kEpsilon)); } @Test @@ -590,15 +598,23 @@ void testOffCenterRotationInverseAccelerations() { () -> assertEquals(expectedAccelerations[3], moduleAccelerations[3].acceleration, kEpsilon), () -> assertEquals( - expectedAngles[0].getDegrees(), moduleAccelerations[0].angle.getDegrees(), kEpsilon), + expectedAngles[0].getDegrees(), + moduleAccelerations[0].angle.getDegrees(), + kEpsilon), () -> assertEquals( - expectedAngles[1].getDegrees(), moduleAccelerations[1].angle.getDegrees(), kEpsilon), + expectedAngles[1].getDegrees(), + moduleAccelerations[1].angle.getDegrees(), + kEpsilon), () -> assertEquals( - expectedAngles[2].getDegrees(), moduleAccelerations[2].angle.getDegrees(), kEpsilon), + expectedAngles[2].getDegrees(), + moduleAccelerations[2].angle.getDegrees(), + kEpsilon), () -> assertEquals( - expectedAngles[3].getDegrees(), moduleAccelerations[3].angle.getDegrees(), kEpsilon)); + expectedAngles[3].getDegrees(), + moduleAccelerations[3].angle.getDegrees(), + kEpsilon)); } } From 39441e7fbbb3b79bd3cc75a260d30f0abfe4acbb Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 13:42:14 -0500 Subject: [PATCH 63/82] style: improve comment readability in SwerveDriveKinematicsTest Signed-off-by: Zach Harel --- .../kinematics/SwerveDriveKinematicsTest.java | 60 ++++++++----------- 1 file changed, 26 insertions(+), 34 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 856b0958e30..baf5a43749c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -147,10 +147,10 @@ void testTurnInPlaceInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); var moduleStates = m_kinematics.toSwerveModuleStates(speeds); - // The circumference of the wheels about the COR is π * diameter, or 2π * radius - // the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels - // trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second, - // our wheels must trace out 1 rotation (or 106.63 inches) per second. + // The circumference of the wheels about the COR is π * diameter, or 2π * radius. The radius + // is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels trace out is + // 106.629190516in. Since we want our robot to rotate at 1 rotation per second, our wheels + // must trace out 1 rotation (or 106.63 inches) per second. assertAll( () -> assertEquals(106.63, moduleStates[0].speed, 0.1), @@ -198,14 +198,12 @@ void testOffCenterCORRotationInverseKinematics() { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl); - // This one is a bit trickier. Because we are rotating about the front-left wheel, - // it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both - // travel - // an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc - // with - // radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel - // should be pointing straight forward, the back-left wheel should be pointing straight right, - // and the back-right wheel should be at a -45 degree angle + // This one is a bit trickier. Because we are rotating about the front-left wheel, it should + // be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel an arc + // with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with + // radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel should + // be pointing straight forward, the back-left wheel should be pointing straight right, and the + // back-right wheel should be at a -45 degree angle. assertAll( () -> assertEquals(0.0, moduleStates[0].speed, 0.1), @@ -228,12 +226,11 @@ void testOffCenterCORRotationForwardKinematics() { var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState); // We already know that our omega should be 2π from the previous test. Next, we need to - // determine - // the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, - // we know that vx and vy must be the same. Furthermore, we know that the center of mass makes - // a full revolution about the center of revolution once every second. Therefore, the center of - // mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are - // 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. + // determine the vx and vy of our chassis center. Because our COR is at a 45 degree angle from + // the center, we know that vx and vy must be the same. Furthermore, we know that the center of + // mass makes a full revolution about the center of revolution once every second. Therefore, + // the center of mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 + // triangle are 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. assertAll( () -> assertEquals(75.398, chassisSpeeds.vx, 0.1), @@ -251,12 +248,11 @@ void testOffCenterCORRotationForwardKinematicsWithDeltas() { var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta); // We already know that our omega should be 2π from the previous test. Next, we need to - // determine - // the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, - // we know that vx and vy must be the same. Furthermore, we know that the center of mass makes - // a full revolution about the center of revolution once every second. Therefore, the center of - // mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triangle are - // 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. + // determine the vx and vy of our chassis center. Because our COR is at a 45 degree angle from + // the center, we know that vx and vy must be the same. Furthermore, we know that the center of + // mass makes a full revolution about the center of revolution once every second. Therefore, + // the center of mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 + // triangle are 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. assertAll( () -> assertEquals(75.398, twist.dx, 0.1), @@ -429,17 +425,13 @@ void testTurnInPlaceInverseAccelerations() { // = √(106.63² + 668.7²) = 678.4 m/s² // // For module positions: - // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = - // -144° - // FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = - // 126° - // BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = - // -54° - // BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = - // 36° + // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = -144° + // FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = 126° + // BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = -54° + // BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = 36° // - // The acceleration angles are not simply tangential because the centripetal component - // from the existing angular velocity dominates and affects the direction. + // The acceleration angles are not simply tangential because the centripetal component from the + // existing angular velocity dominates and affects the direction. double centerRadius = m_kinematics.getModules()[0].getNorm(); double tangentialAccel = centerRadius * accelerations.alpha; // α * r From 8fc64905488741297a8dd94129e3b443ebc34aee Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 13:52:23 -0500 Subject: [PATCH 64/82] style: enhance comment clarity in SwerveDriveKinematicsTest Signed-off-by: Zach Harel --- .../cpp/kinematics/SwerveDriveKinematicsTest.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index 6df80324fb1..878e0f5676b 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -342,12 +342,14 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) { // = √(106.63² + 668.7²) = 678.4 m/s² // // For module positions: - // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = - // -135° → total angle = -144° FR (12, -12): radius angle = 45°, tangential = - // -45°, centripetal = -135° → total angle = 126° BL (-12, 12): radius angle - // = 135°, tangential = 45°, centripetal = 45° → total angle = -54° BR - // (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → - // total angle = 36° + // FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → + // total angle = -144° + // FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → + // total angle = 126° + // BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → + // total angle = -54° + // BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → + // total angle = 36° // // The acceleration angles are not simply tangential because the centripetal // component from the existing angular velocity dominates and affects the From 21dc6be0c03e1934f05798f78696425810d9a7e1 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 15:33:46 -0500 Subject: [PATCH 65/82] fixy part 1 Signed-off-by: Zach Harel --- .../java/org/wpilib/math/kinematics/ChassisAccelerations.java | 2 +- .../src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java | 2 ++ .../{ChassisAccelerations.h => ChassisAccelerations.hpp} | 0 ...lAccelerations.h => DifferentialDriveWheelAccelerations.hpp} | 0 ...eWheelAccelerations.h => MecanumDriveWheelAccelerations.hpp} | 0 ...werveModuleAccelerations.h => SwerveModuleAccelerations.hpp} | 0 ...hassisAccelerationsProto.h => ChassisAccelerationsProto.hpp} | 0 ...ionsProto.h => DifferentialDriveWheelAccelerationsProto.hpp} | 0 ...lerationsProto.h => MecanumDriveWheelAccelerationsProto.hpp} | 0 ...eAccelerationsProto.h => SwerveModuleAccelerationsProto.hpp} | 0 ...ssisAccelerationsStruct.h => ChassisAccelerationsStruct.hpp} | 0 ...nsStruct.h => DifferentialDriveWheelAccelerationsStruct.hpp} | 0 ...rationsStruct.h => MecanumDriveWheelAccelerationsStruct.hpp} | 0 ...ccelerationsStruct.h => SwerveModuleAccelerationsStruct.hpp} | 0 14 files changed, 3 insertions(+), 1 deletion(-) rename wpimath/src/main/native/include/wpi/math/kinematics/{ChassisAccelerations.h => ChassisAccelerations.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/{DifferentialDriveWheelAccelerations.h => DifferentialDriveWheelAccelerations.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/{MecanumDriveWheelAccelerations.h => MecanumDriveWheelAccelerations.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/{SwerveModuleAccelerations.h => SwerveModuleAccelerations.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/proto/{ChassisAccelerationsProto.h => ChassisAccelerationsProto.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/proto/{DifferentialDriveWheelAccelerationsProto.h => DifferentialDriveWheelAccelerationsProto.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/proto/{MecanumDriveWheelAccelerationsProto.h => MecanumDriveWheelAccelerationsProto.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/proto/{SwerveModuleAccelerationsProto.h => SwerveModuleAccelerationsProto.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/struct/{ChassisAccelerationsStruct.h => ChassisAccelerationsStruct.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/struct/{DifferentialDriveWheelAccelerationsStruct.h => DifferentialDriveWheelAccelerationsStruct.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/struct/{MecanumDriveWheelAccelerationsStruct.h => MecanumDriveWheelAccelerationsStruct.hpp} (100%) rename wpimath/src/main/native/include/wpi/math/kinematics/struct/{SwerveModuleAccelerationsStruct.h => SwerveModuleAccelerationsStruct.hpp} (100%) diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java index 714ffe40f32..50ea56a2a7c 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java @@ -7,7 +7,7 @@ import static org.wpilib.units.Units.MetersPerSecondPerSecond; import static org.wpilib.units.Units.RadiansPerSecondPerSecond; -import org.wpilib.math.MathUtil; +import org.wpilib.math.util.MathUtil; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.interpolation.Interpolatable; diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java index 513dbf3f34d..a1e038972a5 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java @@ -11,8 +11,10 @@ import org.wpilib.math.geometry.Transform2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Twist2d; +import org.wpilib.math.interpolation.Interpolatable; import org.wpilib.math.kinematics.proto.ChassisSpeedsProto; import org.wpilib.math.kinematics.struct.ChassisSpeedsStruct; +import org.wpilib.math.util.MathUtil; import org.wpilib.units.measure.AngularVelocity; import org.wpilib.units.measure.LinearVelocity; import org.wpilib.util.protobuf.ProtobufSerializable; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.h b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.h rename to wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.h b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.h rename to wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.h b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.h rename to wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.h b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.h rename to wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.h b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.h rename to wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h rename to wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.h b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.h rename to wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.h b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.h rename to wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.h b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.h rename to wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h rename to wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h rename to wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.h b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp similarity index 100% rename from wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.h rename to wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp From 37eec648368ba5bdf2e378e5cc13e1f7e8b26a2a Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 15:44:15 -0500 Subject: [PATCH 66/82] put the proto files that got eaten back Signed-off-by: Zach Harel --- .../kinematics/proto/ChassisSpeedsProto.hpp | 19 ++++++++ .../DifferentialDriveKinematicsProto.hpp | 24 ++++++++++ .../DifferentialDriveWheelPositionsProto.hpp | 24 ++++++++++ .../DifferentialDriveWheelSpeedsProto.hpp | 24 ++++++++++ .../proto/MecanumDriveKinematicsProto.hpp | 23 +++++++++ .../proto/MecanumDriveWheelPositionsProto.hpp | 24 ++++++++++ .../proto/MecanumDriveWheelSpeedsProto.hpp | 24 ++++++++++ .../proto/SwerveDriveKinematicsProto.hpp | 48 +++++++++++++++++++ .../proto/SwerveModulePositionProto.hpp | 23 +++++++++ .../proto/SwerveModuleStateProto.hpp | 22 +++++++++ 10 files changed, 255 insertions(+) create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisSpeedsProto.hpp create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveKinematicsProto.hpp create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelPositionsProto.hpp create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveKinematicsProto.hpp create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelPositionsProto.hpp create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveDriveKinematicsProto.hpp create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModulePositionProto.hpp create mode 100644 wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleStateProto.hpp diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisSpeedsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisSpeedsProto.hpp new file mode 100644 index 00000000000..0e50b1c8bb0 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisSpeedsProto.hpp @@ -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/math/kinematics/ChassisSpeeds.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT wpi::util::Protobuf { + using MessageStruct = wpi_proto_ProtobufChassisSpeeds; + using InputStream = wpi::util::ProtoInputStream; + using OutputStream = wpi::util::ProtoOutputStream; + static std::optional Unpack(InputStream& stream); + static bool Pack(OutputStream& stream, const wpi::math::ChassisSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveKinematicsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveKinematicsProto.hpp new file mode 100644 index 00000000000..03480b42756 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveKinematicsProto.hpp @@ -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/math/kinematics/DifferentialDriveKinematics.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT + wpi::util::Protobuf { + using MessageStruct = wpi_proto_ProtobufDifferentialDriveKinematics; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::DifferentialDriveKinematics& value); +}; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelPositionsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelPositionsProto.hpp new file mode 100644 index 00000000000..8476a7c2117 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelPositionsProto.hpp @@ -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/math/kinematics/DifferentialDriveWheelPositions.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT + wpi::util::Protobuf { + using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelPositions; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::DifferentialDriveWheelPositions& value); +}; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp new file mode 100644 index 00000000000..f1e59eb00c3 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp @@ -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/math/kinematics/DifferentialDriveWheelSpeeds.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT + wpi::util::Protobuf { + using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelSpeeds; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::DifferentialDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveKinematicsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveKinematicsProto.hpp new file mode 100644 index 00000000000..f98a48112bd --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveKinematicsProto.hpp @@ -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. + +#pragma once + +#include "wpi/math/kinematics/MecanumDriveKinematics.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT wpi::util::Protobuf { + using MessageStruct = wpi_proto_ProtobufMecanumDriveKinematics; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::MecanumDriveKinematics& value); +}; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelPositionsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelPositionsProto.hpp new file mode 100644 index 00000000000..8ae1bc25f9d --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelPositionsProto.hpp @@ -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/math/kinematics/MecanumDriveWheelPositions.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT + wpi::util::Protobuf { + using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelPositions; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::MecanumDriveWheelPositions& value); +}; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp new file mode 100644 index 00000000000..0bdfca42fd0 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp @@ -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/math/kinematics/MecanumDriveWheelSpeeds.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT + wpi::util::Protobuf { + using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelSpeeds; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::MecanumDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveDriveKinematicsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveDriveKinematicsProto.hpp new file mode 100644 index 00000000000..11ddb47d533 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveDriveKinematicsProto.hpp @@ -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. + +#pragma once + +#include + +#include + +#include "wpi/math/kinematics/SwerveDriveKinematics.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpi/util/protobuf/ProtobufCallbacks.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template +struct wpi::util::Protobuf> { + using MessageStruct = wpi_proto_ProtobufSwerveDriveKinematics; + using InputStream = + wpi::util::ProtoInputStream>; + using OutputStream = wpi::util::ProtoOutputStream< + wpi::math::SwerveDriveKinematics>; + + static std::optional> Unpack( + InputStream& stream) { + wpi::util::WpiArrayUnpackCallback + modules; + wpi_proto_ProtobufSwerveDriveKinematics msg{ + .modules = modules.Callback(), + }; + modules.SetLimits(wpi::util::DecodeLimits::Fail); + if (!stream.Decode(msg)) { + return {}; + } + + return wpi::math::SwerveDriveKinematics{modules.Array()}; + } + + static bool Pack(OutputStream& stream, + const wpi::math::SwerveDriveKinematics& value) { + wpi::util::PackCallback modules{ + value.GetModules()}; + wpi_proto_ProtobufSwerveDriveKinematics msg{ + .modules = modules.Callback(), + }; + return stream.Encode(msg); + } +}; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModulePositionProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModulePositionProto.hpp new file mode 100644 index 00000000000..63812cc6a51 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModulePositionProto.hpp @@ -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. + +#pragma once + +#include "wpi/math/kinematics/SwerveModulePosition.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT wpi::util::Protobuf { + using MessageStruct = wpi_proto_ProtobufSwerveModulePosition; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::SwerveModulePosition& value); +}; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleStateProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleStateProto.hpp new file mode 100644 index 00000000000..150b0a30d5a --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleStateProto.hpp @@ -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/math/kinematics/SwerveModuleState.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" +#include "wpimath/protobuf/kinematics.npb.h" + +template <> +struct WPILIB_DLLEXPORT wpi::util::Protobuf { + using MessageStruct = wpi_proto_ProtobufSwerveModuleState; + using InputStream = wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::SwerveModuleState& value); +}; From edadb2faef0c61d8175b3418280971c12e1ff1c5 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 16:31:36 -0500 Subject: [PATCH 67/82] fixy part 2 Signed-off-by: Zach Harel --- .../cpp/kinematics/MecanumDriveKinematics.cpp | 7 ++-- .../proto/ChassisAccelerationsProto.cpp | 12 +++---- ...fferentialDriveWheelAccelerationsProto.cpp | 12 +++---- .../MecanumDriveWheelAccelerationsProto.cpp | 12 +++---- .../proto/SwerveModuleAccelerationsProto.cpp | 18 +++++------ .../struct/ChassisAccelerationsStruct.cpp | 26 +++++++-------- ...ferentialDriveWheelAccelerationsStruct.cpp | 24 +++++++------- .../MecanumDriveWheelAccelerationsStruct.cpp | 32 +++++++++---------- .../SwerveModuleAccelerationsStruct.cpp | 24 +++++++------- .../estimator/SwerveDrivePoseEstimator.hpp | 7 ++-- .../math/kinematics/ChassisAccelerations.hpp | 18 +++++------ .../DifferentialDriveKinematics.hpp | 2 ++ .../DifferentialDriveWheelAccelerations.hpp | 8 ++--- .../wpi/math/kinematics/Kinematics.hpp | 1 + .../kinematics/MecanumDriveKinematics.hpp | 2 ++ .../MecanumDriveWheelAccelerations.hpp | 8 ++--- .../math/kinematics/SwerveDriveKinematics.hpp | 21 ++++++------ .../math/kinematics/SwerveDriveOdometry3d.hpp | 7 ++-- .../kinematics/SwerveModuleAccelerations.hpp | 8 ++--- .../proto/ChassisAccelerationsProto.hpp | 10 +++--- ...fferentialDriveWheelAccelerationsProto.hpp | 12 +++---- .../MecanumDriveWheelAccelerationsProto.hpp | 12 +++---- .../proto/SwerveModuleAccelerationsProto.hpp | 12 +++---- .../struct/ChassisAccelerationsStruct.hpp | 14 ++++---- ...ferentialDriveWheelAccelerationsStruct.hpp | 10 +++--- .../MecanumDriveWheelAccelerationsStruct.hpp | 10 +++--- .../SwerveModuleAccelerationsStruct.hpp | 16 +++++----- .../kinematics/ChassisAccelerationsTest.cpp | 9 +++--- .../DifferentialDriveKinematicsTest.cpp | 7 ++-- ...ifferentialDriveWheelAccelerationsTest.cpp | 7 ++-- .../kinematics/MecanumDriveKinematicsTest.cpp | 2 +- .../MecanumDriveWheelAccelerationsTest.cpp | 7 ++-- .../kinematics/SwerveDriveKinematicsTest.cpp | 6 ++-- .../SwerveModuleAccelerationsTest.cpp | 7 ++-- 34 files changed, 203 insertions(+), 187 deletions(-) diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index 0fe48f483fe..1e9d9540280 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -99,9 +99,10 @@ ChassisAccelerations MecanumDriveKinematics::ToChassisAccelerations( Eigen::Vector3d chassisAccelerationsVector = m_forwardKinematics.solve(wheelAccelerationsVector); - return {wpi::units::meters_per_second_squared_t{chassisAccelerationsVector(0)}, - wpi::units::meters_per_second_squared_t{chassisAccelerationsVector(1)}, - wpi::units::radians_per_second_squared_t{chassisAccelerationsVector(2)}}; + return { + wpi::units::meters_per_second_squared_t{chassisAccelerationsVector(0)}, + wpi::units::meters_per_second_squared_t{chassisAccelerationsVector(1)}, + wpi::units::radians_per_second_squared_t{chassisAccelerationsVector(2)}}; } MecanumDriveWheelAccelerations MecanumDriveKinematics::ToWheelAccelerations( diff --git a/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp index 425e78730ba..2052c6122a8 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp @@ -2,26 +2,26 @@ // 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/kinematics/proto/ChassisAccelerationsProto.h" +#include "wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp" #include "wpimath/protobuf/kinematics.npb.h" -std::optional -wpi::Protobuf::Unpack(InputStream& stream) { +std::optional +wpi::util::Protobuf::Unpack(InputStream& stream) { wpi_proto_ProtobufChassisAccelerations msg; if (!stream.Decode(msg)) { return {}; } - return frc::ChassisAccelerations{ + return wpi::math::ChassisAccelerations{ units::meters_per_second_squared_t{msg.ax}, units::meters_per_second_squared_t{msg.ay}, units::radians_per_second_squared_t{msg.alpha}, }; } -bool wpi::Protobuf::Pack( - OutputStream& stream, const frc::ChassisAccelerations& value) { +bool wpi::util::Protobuf::Pack( + OutputStream& stream, const wpi::math::ChassisAccelerations& value) { wpi_proto_ProtobufChassisAccelerations msg{ .ax = value.ax.value(), .ay = value.ay.value(), diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp index 0148dfabd56..754be6bf037 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp @@ -2,26 +2,26 @@ // 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/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h" +#include "wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp" #include "wpimath/protobuf/kinematics.npb.h" -std::optional wpi::Protobuf< - frc::DifferentialDriveWheelAccelerations>::Unpack(InputStream& stream) { +std::optional wpi::util::Protobuf< + wpi::math::DifferentialDriveWheelAccelerations>::Unpack(InputStream& stream) { wpi_proto_ProtobufDifferentialDriveWheelAccelerations msg; if (!stream.Decode(msg)) { return {}; } - return frc::DifferentialDriveWheelAccelerations{ + return wpi::math::DifferentialDriveWheelAccelerations{ units::meters_per_second_squared_t{msg.left}, units::meters_per_second_squared_t{msg.right}, }; } -bool wpi::Protobuf::Pack( +bool wpi::util::Protobuf::Pack( OutputStream& stream, - const frc::DifferentialDriveWheelAccelerations& value) { + const wpi::math::DifferentialDriveWheelAccelerations& value) { wpi_proto_ProtobufDifferentialDriveWheelAccelerations msg{ .left = value.left.value(), .right = value.right.value(), diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp index e024ee97e5c..21066134538 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp @@ -2,18 +2,18 @@ // 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/kinematics/proto/MecanumDriveWheelAccelerationsProto.h" +#include "wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp" #include "wpimath/protobuf/kinematics.npb.h" -std::optional wpi::Protobuf< - frc::MecanumDriveWheelAccelerations>::Unpack(InputStream& stream) { +std::optional wpi::util::Protobuf< + wpi::math::MecanumDriveWheelAccelerations>::Unpack(InputStream& stream) { wpi_proto_ProtobufMecanumDriveWheelAccelerations msg; if (!stream.Decode(msg)) { return {}; } - return frc::MecanumDriveWheelAccelerations{ + return wpi::math::MecanumDriveWheelAccelerations{ units::meters_per_second_squared_t{msg.front_left}, units::meters_per_second_squared_t{msg.front_right}, units::meters_per_second_squared_t{msg.rear_left}, @@ -21,8 +21,8 @@ std::optional wpi::Protobuf< }; } -bool wpi::Protobuf::Pack( - OutputStream& stream, const frc::MecanumDriveWheelAccelerations& value) { +bool wpi::util::Protobuf::Pack( + OutputStream& stream, const wpi::math::MecanumDriveWheelAccelerations& value) { wpi_proto_ProtobufMecanumDriveWheelAccelerations msg{ .front_left = value.frontLeft.value(), .front_right = value.frontRight.value(), diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp index 6b9a5c73f3c..9477e3d3fe9 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp @@ -2,15 +2,15 @@ // 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/kinematics/proto/SwerveModuleAccelerationsProto.h" +#include "wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp" -#include +#include #include "wpimath/protobuf/kinematics.npb.h" -std::optional -wpi::Protobuf::Unpack(InputStream& stream) { - wpi::UnpackCallback angle; +std::optional +wpi::util::Protobuf::Unpack(InputStream& stream) { + wpi::util::UnpackCallback angle; wpi_proto_ProtobufSwerveModuleAccelerations msg{ .acceleration = 0, .angle = angle.Callback(), @@ -25,15 +25,15 @@ wpi::Protobuf::Unpack(InputStream& stream) { return {}; } - return frc::SwerveModuleAccelerations{ + return wpi::math::SwerveModuleAccelerations{ units::meters_per_second_squared_t{msg.acceleration}, iangle[0], }; } -bool wpi::Protobuf::Pack( - OutputStream& stream, const frc::SwerveModuleAccelerations& value) { - wpi::PackCallback angle{&value.angle}; +bool wpi::util::Protobuf::Pack( + OutputStream& stream, const wpi::math::SwerveModuleAccelerations& value) { + wpi::util::PackCallback angle{&value.angle}; wpi_proto_ProtobufSwerveModuleAccelerations msg{ .acceleration = value.acceleration.value(), .angle = angle.Callback(), diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp index a522d910ee6..f6deb09e7e0 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp @@ -2,33 +2,33 @@ // 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/kinematics/struct/ChassisAccelerationsStruct.h" +#include "wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp" -#include +#include -#include "frc/kinematics/ChassisAccelerations.h" +#include "wpi/math/kinematics/ChassisAccelerations.hpp" -frc::ChassisAccelerations wpi::Struct::Unpack( +wpi::math::ChassisAccelerations wpi::util::Struct::Unpack( std::span data) { constexpr size_t kAxOff = 0; constexpr size_t kAyOff = kAxOff + 8; constexpr size_t kAlphaOff = kAyOff + 8; - return frc::ChassisAccelerations{ + return wpi::math::ChassisAccelerations{ units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}, units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}, units::radians_per_second_squared_t{ - wpi::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}, }; } -void wpi::Struct::Pack( - std::span data, const frc::ChassisAccelerations& value) { +void wpi::util::Struct::Pack( + std::span data, const wpi::math::ChassisAccelerations& value) { constexpr size_t kAxOff = 0; constexpr size_t kAyOff = kAxOff + 8; constexpr size_t kAlphaOff = kAyOff + 8; - wpi::PackStruct(data, value.ax.value()); - wpi::PackStruct(data, value.ay.value()); - wpi::PackStruct(data, value.alpha.value()); + wpi::util::PackStruct(data, value.ax.value()); + wpi::util::PackStruct(data, value.ay.value()); + wpi::util::PackStruct(data, value.alpha.value()); } diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp index 40867e5a4db..215e32c49a8 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp @@ -2,30 +2,30 @@ // 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/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h" +#include "wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp" -#include +#include -#include "frc/kinematics/DifferentialDriveWheelAccelerations.h" +#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" -frc::DifferentialDriveWheelAccelerations -wpi::Struct::Unpack( +wpi::math::DifferentialDriveWheelAccelerations +wpi::util::Struct::Unpack( std::span data) { constexpr size_t kLeftOff = 0; constexpr size_t kRightOff = kLeftOff + 8; - return frc::DifferentialDriveWheelAccelerations{ + return wpi::math::DifferentialDriveWheelAccelerations{ units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}, units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}, }; } -void wpi::Struct::Pack( +void wpi::util::Struct::Pack( std::span data, - const frc::DifferentialDriveWheelAccelerations& value) { + const wpi::math::DifferentialDriveWheelAccelerations& value) { constexpr size_t kLeftOff = 0; constexpr size_t kRightOff = kLeftOff + 8; - wpi::PackStruct(data, value.left.value()); - wpi::PackStruct(data, value.right.value()); + wpi::util::PackStruct(data, value.left.value()); + wpi::util::PackStruct(data, value.right.value()); } diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp index 6ef8e5c4eb6..89c65ff7eaf 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp @@ -2,39 +2,39 @@ // 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/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h" +#include "wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp" -#include +#include -#include "frc/kinematics/MecanumDriveWheelAccelerations.h" +#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" -frc::MecanumDriveWheelAccelerations -wpi::Struct::Unpack( +wpi::math::MecanumDriveWheelAccelerations +wpi::util::Struct::Unpack( std::span data) { constexpr size_t kFrontLeftOff = 0; constexpr size_t kFrontRightOff = kFrontLeftOff + 8; constexpr size_t kRearLeftOff = kFrontRightOff + 8; constexpr size_t kRearRightOff = kRearLeftOff + 8; - return frc::MecanumDriveWheelAccelerations{ + return wpi::math::MecanumDriveWheelAccelerations{ units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}, units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}, units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}, units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}, }; } -void wpi::Struct::Pack( - std::span data, const frc::MecanumDriveWheelAccelerations& value) { +void wpi::util::Struct::Pack( + std::span data, const wpi::math::MecanumDriveWheelAccelerations& value) { constexpr size_t kFrontLeftOff = 0; constexpr size_t kFrontRightOff = kFrontLeftOff + 8; constexpr size_t kRearLeftOff = kFrontRightOff + 8; constexpr size_t kRearRightOff = kRearLeftOff + 8; - wpi::PackStruct(data, value.frontLeft.value()); - wpi::PackStruct(data, value.frontRight.value()); - wpi::PackStruct(data, value.rearLeft.value()); - wpi::PackStruct(data, value.rearRight.value()); + wpi::util::PackStruct(data, value.frontLeft.value()); + wpi::util::PackStruct(data, value.frontRight.value()); + wpi::util::PackStruct(data, value.rearLeft.value()); + wpi::util::PackStruct(data, value.rearRight.value()); } diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp index e3217bfc459..2b9a2e5139b 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp @@ -2,26 +2,26 @@ // 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/kinematics/struct/SwerveModuleAccelerationsStruct.h" +#include "wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp" -#include +#include -#include "frc/kinematics/SwerveModuleAccelerations.h" +#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" -frc::SwerveModuleAccelerations wpi::Struct< - frc::SwerveModuleAccelerations>::Unpack(std::span data) { +wpi::math::SwerveModuleAccelerations wpi::util::Struct< + wpi::math::SwerveModuleAccelerations>::Unpack(std::span data) { constexpr size_t kAccelerationOff = 0; constexpr size_t kAngleOff = kAccelerationOff + 8; - return frc::SwerveModuleAccelerations{ + return wpi::math::SwerveModuleAccelerations{ units::meters_per_second_squared_t{ - wpi::UnpackStruct(data)}, - wpi::UnpackStruct(data)}; + wpi::util::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}; } -void wpi::Struct::Pack( - std::span data, const frc::SwerveModuleAccelerations& value) { +void wpi::util::Struct::Pack( + std::span data, const wpi::math::SwerveModuleAccelerations& value) { constexpr size_t kAccelerationOff = 0; constexpr size_t kAngleOff = kAccelerationOff + 8; - wpi::PackStruct(data, value.acceleration.value()); - wpi::PackStruct(data, value.angle); + wpi::util::PackStruct(data, value.acceleration.value()); + wpi::util::PackStruct(data, value.angle); } diff --git a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp index b827b36dc0b..9ae0922bded 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp @@ -30,9 +30,10 @@ namespace wpi::math { */ template class SwerveDrivePoseEstimator - : public PoseEstimator, - wpi::util::array, - wpi::util::array> { + : public PoseEstimator< + wpi::util::array, + wpi::util::array, + wpi::util::array> { public: /** * Constructs a SwerveDrivePoseEstimator with default standard deviations diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp index 2bde2430512..b9df527a05c 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp @@ -6,13 +6,13 @@ #include -#include "wpi/math/MathUtil.h" -#include "wpi/math/geometry/Rotation2d.h" -#include "wpi/math/geometry/Translation2d.h" -#include "wpi/units/acceleration.h" -#include "wpi/units/angular_acceleration.h" +#include "wpi/math/util/MathUtil.hpp" +#include "wpi/math/geometry/Rotation2d.hpp" +#include "wpi/math/geometry/Translation2d.hpp" +#include "wpi/units/acceleration.hpp" +#include "wpi/units/angular_acceleration.hpp" -namespace frc { +namespace wpi::math { /** * Represents the acceleration of a robot chassis. Although this struct contains * similar members compared to a ChassisSpeeds, they do NOT represent the same @@ -161,7 +161,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations { */ constexpr bool operator==(const ChassisAccelerations& other) const = default; }; -} // namespace frc +} // namespace wpi::math -#include "frc/kinematics/proto/ChassisAccelerationsProto.h" -#include "frc/kinematics/struct/ChassisAccelerationsStruct.h" +#include "wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp" +#include "wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp index 24a5adae5ae..7e9c82b77a8 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp @@ -7,7 +7,9 @@ #include #include "wpi/math/geometry/Twist2d.hpp" +#include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpi/math/kinematics/ChassisSpeeds.hpp" +#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" #include "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp" #include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp" #include "wpi/math/kinematics/Kinematics.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp index 7f131da44f2..edd96bb97ff 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp @@ -9,7 +9,7 @@ #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" -namespace frc { +namespace wpi::math { /** * Represents the wheel accelerations for a differential drive drivetrain. */ @@ -108,7 +108,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations { constexpr bool operator==( const DifferentialDriveWheelAccelerations& other) const = default; }; -} // namespace frc +} // namespace wpi::math -#include "frc/kinematics/proto/DifferentialDriveWheelAccelerationsProto.h" -#include "frc/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.h" +#include "wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp" +#include "wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp index f4e16c546fd..b8bae103535 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp @@ -5,6 +5,7 @@ #pragma once #include "wpi/math/geometry/Twist2d.hpp" +#include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpi/math/kinematics/ChassisSpeeds.hpp" #include "wpi/util/SymbolExports.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp index 3746b2ce5eb..6f59e841bd5 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp @@ -8,8 +8,10 @@ #include "wpi/math/geometry/Translation2d.hpp" #include "wpi/math/geometry/Twist2d.hpp" +#include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpi/math/kinematics/ChassisSpeeds.hpp" #include "wpi/math/kinematics/Kinematics.hpp" +#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" #include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp" #include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp" #include "wpi/math/linalg/EigenCore.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp index e260efd1f34..d26ef1e1dff 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp @@ -13,7 +13,7 @@ #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" -namespace frc { +namespace wpi::math { /** * Represents the wheel accelerations for a mecanum drive drivetrain. */ @@ -120,7 +120,7 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations { constexpr bool operator==(const MecanumDriveWheelAccelerations& other) const = default; }; -} // namespace frc +} // namespace wpi::math -#include "frc/kinematics/proto/MecanumDriveWheelAccelerationsProto.h" -#include "frc/kinematics/struct/MecanumDriveWheelAccelerationsStruct.h" +#include "wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp" +#include "wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp index b63384c5c24..8eb4a28e91c 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp @@ -13,8 +13,10 @@ #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/math/geometry/Translation2d.hpp" #include "wpi/math/geometry/Twist2d.hpp" +#include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpi/math/kinematics/ChassisSpeeds.hpp" #include "wpi/math/kinematics/Kinematics.hpp" +#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" #include "wpi/math/kinematics/SwerveModulePosition.hpp" #include "wpi/math/kinematics/SwerveModuleState.hpp" #include "wpi/math/linalg/EigenCore.hpp" @@ -50,9 +52,10 @@ namespace wpi::math { */ template class SwerveDriveKinematics - : public Kinematics, - wpi::util::array, - wpi::util::array> { + : public Kinematics< + wpi::util::array, + wpi::util::array, + wpi::util::array> { public: /** * Constructs a swerve drive kinematics object. This takes in a variable @@ -472,7 +475,7 @@ class SwerveDriveKinematics * that corner. * @return An array containing the module accelerations. */ - wpi::array ToSwerveModuleAccelerations( + wpi::util::array ToSwerveModuleAccelerations( const ChassisAccelerations& chassisAccelerations, const units::radians_per_second_t angularVelocity = 0.0_rad_per_s, const Translation2d& centerOfRotation = Translation2d{}) const { @@ -480,8 +483,8 @@ class SwerveDriveKinematics // Kinematics" by FRC Team 449 - The Blair Robot Project, Rafi Pedersen // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf - wpi::array moduleAccelerations( - wpi::empty_array); + wpi::util::array moduleAccelerations( + wpi::util::empty_array); if (chassisAccelerations.ax == 0.0_mps_sq && chassisAccelerations.ay == 0.0_mps_sq && @@ -535,7 +538,7 @@ class SwerveDriveKinematics * @param chassisAccelerations The desired chassis accelerations. * @return An array containing the module accelerations. */ - wpi::array ToWheelAccelerations( + wpi::util::array ToWheelAccelerations( const ChassisAccelerations& chassisAccelerations) const override { return ToSwerveModuleAccelerations(chassisAccelerations); } @@ -557,7 +560,7 @@ class SwerveDriveKinematics ChassisAccelerations ToChassisAccelerations( ModuleAccelerations&&... moduleAccelerations) const { return this->ToChassisAccelerations( - wpi::array{ + wpi::util::array{ moduleAccelerations...}); } @@ -573,7 +576,7 @@ class SwerveDriveKinematics * @return The resulting chassis accelerations. */ ChassisAccelerations ToChassisAccelerations( - const wpi::array& + const wpi::util::array& moduleAccelerations) const override { // Derivation for second-order kinematics from "Swerve Drive Second Order // Kinematics" by FRC Team 449 - The Blair Robot Project, Rafi Pedersen diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp index a2f3deafdc3..d05cf3f1143 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp @@ -30,9 +30,10 @@ namespace wpi::math { */ template class SwerveDriveOdometry3d - : public Odometry3d, - wpi::util::array, - wpi::util::array> { + : public Odometry3d< + wpi::util::array, + wpi::util::array, + wpi::util::array> { public: /** * Constructs a SwerveDriveOdometry3d object. diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp index b9a58bae586..c37bc2a7b85 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp @@ -10,7 +10,7 @@ #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" -namespace frc { +namespace wpi::math { /** * Represents the accelerations of one swerve module. */ @@ -107,7 +107,7 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { return operator*(1.0 / scalar); } }; -} // namespace frc +} // namespace wpi::math -#include "frc/kinematics/proto/SwerveModuleAccelerationsProto.h" -#include "frc/kinematics/struct/SwerveModuleAccelerationsStruct.h" +#include "wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp" +#include "wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp index 7c0ee4f1002..dfbd0aeabdb 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp @@ -11,11 +11,11 @@ #include "wpimath/protobuf/kinematics.npb.h" template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { +struct WPILIB_DLLEXPORT wpi::util::Protobuf { using MessageStruct = wpi_proto_ProtobufChassisAccelerations; - using InputStream = wpi::ProtoInputStream; - using OutputStream = wpi::ProtoOutputStream; - static std::optional Unpack(InputStream& stream); + using InputStream = wpi::util::ProtoInputStream; + using OutputStream = wpi::util::ProtoOutputStream; + static std::optional Unpack(InputStream& stream); static bool Pack(OutputStream& stream, - const frc::ChassisAccelerations& value); + const wpi::math::ChassisAccelerations& value); }; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp index 1b5c780547c..2db01115ccd 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp @@ -7,19 +7,19 @@ #include #include -#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.h" +#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" template <> struct WPILIB_DLLEXPORT - wpi::Protobuf { + wpi::util::Protobuf { using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelAccelerations; using InputStream = - wpi::ProtoInputStream; + wpi::util::ProtoInputStream; using OutputStream = - wpi::ProtoOutputStream; - static std::optional Unpack( + wpi::util::ProtoOutputStream; + static std::optional Unpack( InputStream& stream); static bool Pack(OutputStream& stream, - const frc::DifferentialDriveWheelAccelerations& value); + const wpi::math::DifferentialDriveWheelAccelerations& value); }; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp index 5657c7aaa32..a21403e0c24 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp @@ -7,18 +7,18 @@ #include #include -#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.h" +#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { +struct WPILIB_DLLEXPORT wpi::util::Protobuf { using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelAccelerations; using InputStream = - wpi::ProtoInputStream; + wpi::util::ProtoInputStream; using OutputStream = - wpi::ProtoOutputStream; - static std::optional Unpack( + wpi::util::ProtoOutputStream; + static std::optional Unpack( InputStream& stream); static bool Pack(OutputStream& stream, - const frc::MecanumDriveWheelAccelerations& value); + const wpi::math::MecanumDriveWheelAccelerations& value); }; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp index 3f8ad6e34c7..39e1f9b61a2 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp @@ -7,16 +7,16 @@ #include #include -#include "wpi/math/kinematics/SwerveModuleAccelerations.h" +#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { +struct WPILIB_DLLEXPORT wpi::util::Protobuf { using MessageStruct = wpi_proto_ProtobufSwerveModuleAccelerations; - using InputStream = wpi::ProtoInputStream; - using OutputStream = wpi::ProtoOutputStream; - static std::optional Unpack( + using InputStream = wpi::util::ProtoInputStream; + using OutputStream = wpi::util::ProtoOutputStream; + static std::optional Unpack( InputStream& stream); static bool Pack(OutputStream& stream, - const frc::SwerveModuleAccelerations& value); + const wpi::math::SwerveModuleAccelerations& value); }; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp index 629af541b3a..1ae4d85c618 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp @@ -4,13 +4,13 @@ #pragma once -#include -#include +#include +#include -#include "wpi/math/kinematics/ChassisAccelerations.h" +#include "wpi/math/kinematics/ChassisAccelerations.hpp" template <> -struct WPILIB_DLLEXPORT wpi::Struct { +struct WPILIB_DLLEXPORT wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "ChassisAccelerations"; } @@ -19,9 +19,9 @@ struct WPILIB_DLLEXPORT wpi::Struct { return "double ax;double ay;double alpha"; } - static frc::ChassisAccelerations Unpack(std::span data); + static wpi::math::ChassisAccelerations Unpack(std::span data); static void Pack(std::span data, - const frc::ChassisAccelerations& value); + const wpi::math::ChassisAccelerations& value); }; -static_assert(wpi::StructSerializable); +static_assert(wpi::util::StructSerializable); diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp index cb9b57cb839..78538bfc8e1 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp @@ -7,10 +7,10 @@ #include #include -#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.h" +#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" template <> -struct WPILIB_DLLEXPORT wpi::Struct { +struct WPILIB_DLLEXPORT wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "DifferentialDriveWheelAccelerations"; } @@ -19,11 +19,11 @@ struct WPILIB_DLLEXPORT wpi::Struct { return "double left;double right"; } - static frc::DifferentialDriveWheelAccelerations Unpack( + static wpi::math::DifferentialDriveWheelAccelerations Unpack( std::span data); static void Pack(std::span data, - const frc::DifferentialDriveWheelAccelerations& value); + const wpi::math::DifferentialDriveWheelAccelerations& value); }; static_assert( - wpi::StructSerializable); + wpi::util::StructSerializable); diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp index d674fbf0bea..7b4e18fc1ea 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp @@ -7,10 +7,10 @@ #include #include -#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.h" +#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" template <> -struct WPILIB_DLLEXPORT wpi::Struct { +struct WPILIB_DLLEXPORT wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "MecanumDriveWheelAccelerations"; } @@ -20,10 +20,10 @@ struct WPILIB_DLLEXPORT wpi::Struct { "rear_right"; } - static frc::MecanumDriveWheelAccelerations Unpack( + static wpi::math::MecanumDriveWheelAccelerations Unpack( std::span data); static void Pack(std::span data, - const frc::MecanumDriveWheelAccelerations& value); + const wpi::math::MecanumDriveWheelAccelerations& value); }; -static_assert(wpi::StructSerializable); +static_assert(wpi::util::StructSerializable); diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp index aba24f567d1..7f899ef88e1 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp @@ -7,28 +7,28 @@ #include #include -#include "wpi/math/kinematics/SwerveModuleAccelerations.h" +#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" template <> -struct WPILIB_DLLEXPORT wpi::Struct { +struct WPILIB_DLLEXPORT wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "SwerveModuleAccelerations"; } static constexpr size_t GetSize() { - return 8 + wpi::GetStructSize(); + return 8 + wpi::util::GetStructSize(); } static constexpr std::string_view GetSchema() { return "double acceleration;Rotation2d angle"; } - static frc::SwerveModuleAccelerations Unpack(std::span data); + static wpi::math::SwerveModuleAccelerations Unpack(std::span data); static void Pack(std::span data, - const frc::SwerveModuleAccelerations& value); + const wpi::math::SwerveModuleAccelerations& value); static void ForEachNested( std::invocable auto fn) { - wpi::ForEachStructSchema(fn); + wpi::util::ForEachStructSchema(fn); } }; -static_assert(wpi::StructSerializable); -static_assert(wpi::HasNestedStruct); +static_assert(wpi::util::StructSerializable); +static_assert(wpi::util::HasNestedStruct); diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp index 5470df8d8cb..909e9a2065b 100644 --- a/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp @@ -2,15 +2,16 @@ // 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 "wpi/math/kinematics/ChassisAccelerations.hpp" + #include #include -#include "frc/kinematics/ChassisAccelerations.h" -#include "units/acceleration.h" -#include "units/angular_acceleration.h" +#include "wpi/units/acceleration.hpp" +#include "wpi/units/angular_acceleration.hpp" -using namespace frc; +using namespace wpi::math; static constexpr double kEpsilon = 1E-9; diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp index 9a1bd740405..542f7d836b6 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp @@ -9,6 +9,7 @@ #include #include "wpi/math/kinematics/ChassisSpeeds.hpp" +#include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpi/units/angular_velocity.hpp" #include "wpi/units/length.hpp" #include "wpi/units/velocity.hpp" @@ -125,7 +126,7 @@ TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForRotateInPlace) { const DifferentialDriveKinematics kinematics{0.381_m * 2}; const ChassisAccelerations chassisAccelerations{ 0.0_mps_sq, 0.0_mps_sq, - units::radians_per_second_squared_t{std::numbers::pi}}; + wpi::units::radians_per_second_squared_t{std::numbers::pi}}; const auto wheelAccelerations = kinematics.ToWheelAccelerations(chassisAccelerations); @@ -138,8 +139,8 @@ TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForRotateInPlace) { TEST(DifferentialDriveKinematicsTest, ForwardAccelerationsForRotateInPlace) { const DifferentialDriveKinematics kinematics{0.381_m * 2}; const DifferentialDriveWheelAccelerations wheelAccelerations{ - units::meters_per_second_squared_t{+0.381 * std::numbers::pi}, - units::meters_per_second_squared_t{-0.381 * std::numbers::pi}}; + wpi::units::meters_per_second_squared_t{+0.381 * std::numbers::pi}, + wpi::units::meters_per_second_squared_t{-0.381 * std::numbers::pi}}; const auto chassisAccelerations = kinematics.ToChassisAccelerations(wheelAccelerations); diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp index 4be9485aaf9..9393df18a51 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp @@ -2,12 +2,13 @@ // 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 "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" + #include -#include "frc/kinematics/DifferentialDriveWheelAccelerations.h" -#include "units/acceleration.h" +#include "wpi/units/acceleration.hpp" -using namespace frc; +using namespace wpi::math; static constexpr double kEpsilon = 1E-9; diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index 0a501c5b358..9fcc551fb7d 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -274,7 +274,7 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardAccelerations) { TEST_F(MecanumDriveKinematicsTest, RotationInverseAccelerations) { ChassisAccelerations accelerations{ 0_mps_sq, 0_mps_sq, - units::radians_per_second_squared_t{2 * std::numbers::pi}}; + wpi::units::radians_per_second_squared_t{2 * std::numbers::pi}}; auto wheelAccelerations = kinematics.ToWheelAccelerations(accelerations); EXPECT_NEAR(-150.79645, wheelAccelerations.frontLeft.value(), 0.1); diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp index 32c94221e68..591a2df6fea 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp @@ -2,12 +2,13 @@ // 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 "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" + #include -#include "frc/kinematics/MecanumDriveWheelAccelerations.h" -#include "units/acceleration.h" +#include "wpi/units/acceleration.hpp" -using namespace frc; +using namespace wpi::math; static constexpr double kEpsilon = 1E-9; diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index a8d4cb5b79f..4537258ac76 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -314,8 +314,8 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) { TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) { ChassisAccelerations accelerations{ 0_mps_sq, 0_mps_sq, - units::radians_per_second_squared_t{2 * std::numbers::pi}}; - units::radians_per_second_t angularVelocity = 2_rad_per_s * std::numbers::pi; + wpi::units::radians_per_second_squared_t{2 * std::numbers::pi}}; + wpi::units::radians_per_second_t angularVelocity = 2_rad_per_s * std::numbers::pi; auto [flAccel, frAccel, blAccel, brAccel] = m_kinematics.ToSwerveModuleAccelerations(accelerations, angularVelocity); @@ -413,7 +413,7 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardAccelerations) { TEST_F(SwerveDriveKinematicsTest, OffCenterRotationInverseAccelerations) { ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, 1_rad_per_s_sq}; // For this test, assume an angular velocity of 1 rad/s - units::radians_per_second_t angularVelocity = 1.0_rad_per_s; + wpi::units::radians_per_second_t angularVelocity = 1.0_rad_per_s; auto [flAccel, frAccel, blAccel, brAccel] = m_kinematics.ToSwerveModuleAccelerations(accelerations, angularVelocity, m_fl); diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp index 386070fd8e4..4a925e40311 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp @@ -2,14 +2,15 @@ // 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 "wpi/math/kinematics/SwerveModuleAccelerations.hpp" + #include #include -#include "frc/kinematics/SwerveModuleAccelerations.h" -#include "units/acceleration.h" +#include "wpi/units/acceleration.hpp" -using namespace frc; +using namespace wpi::math; static constexpr double kEpsilon = 1E-9; From 2deddf24dc85d911f313cd40b5c57320f6fb5274 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 16:35:53 -0500 Subject: [PATCH 68/82] fixy part 3: wpiformat Signed-off-by: Zach Harel --- .../cpp/kinematics/proto/ChassisAccelerationsProto.cpp | 4 ++-- .../proto/DifferentialDriveWheelAccelerationsProto.cpp | 5 +++-- .../proto/MecanumDriveWheelAccelerationsProto.cpp | 3 ++- .../kinematics/proto/SwerveModuleAccelerationsProto.cpp | 4 ++-- .../cpp/kinematics/struct/ChassisAccelerationsStruct.cpp | 4 ++-- .../struct/MecanumDriveWheelAccelerationsStruct.cpp | 3 ++- .../struct/SwerveModuleAccelerationsStruct.cpp | 8 +++++--- .../include/wpi/math/kinematics/ChassisAccelerations.hpp | 2 +- .../wpi/math/kinematics/SwerveDriveKinematics.hpp | 3 ++- .../math/kinematics/proto/ChassisAccelerationsProto.hpp | 9 ++++++--- .../proto/DifferentialDriveWheelAccelerationsProto.hpp | 8 ++++---- .../proto/MecanumDriveWheelAccelerationsProto.hpp | 3 ++- .../kinematics/proto/SwerveModuleAccelerationsProto.hpp | 9 ++++++--- .../struct/DifferentialDriveWheelAccelerationsStruct.hpp | 7 ++++--- .../struct/MecanumDriveWheelAccelerationsStruct.hpp | 6 ++++-- .../struct/SwerveModuleAccelerationsStruct.hpp | 9 ++++++--- .../cpp/kinematics/DifferentialDriveKinematicsTest.cpp | 2 +- .../native/cpp/kinematics/SwerveDriveKinematicsTest.cpp | 3 ++- 18 files changed, 56 insertions(+), 36 deletions(-) diff --git a/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp index 2052c6122a8..b573d60ff5e 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/ChassisAccelerationsProto.cpp @@ -6,8 +6,8 @@ #include "wpimath/protobuf/kinematics.npb.h" -std::optional -wpi::util::Protobuf::Unpack(InputStream& stream) { +std::optional wpi::util::Protobuf< + wpi::math::ChassisAccelerations>::Unpack(InputStream& stream) { wpi_proto_ProtobufChassisAccelerations msg; if (!stream.Decode(msg)) { return {}; diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp index 754be6bf037..b39542fdc32 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp @@ -6,8 +6,9 @@ #include "wpimath/protobuf/kinematics.npb.h" -std::optional wpi::util::Protobuf< - wpi::math::DifferentialDriveWheelAccelerations>::Unpack(InputStream& stream) { +std::optional +wpi::util::Protobuf::Unpack( + InputStream& stream) { wpi_proto_ProtobufDifferentialDriveWheelAccelerations msg; if (!stream.Decode(msg)) { return {}; diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp index 21066134538..7311ac426fc 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.cpp @@ -22,7 +22,8 @@ std::optional wpi::util::Protobuf< } bool wpi::util::Protobuf::Pack( - OutputStream& stream, const wpi::math::MecanumDriveWheelAccelerations& value) { + OutputStream& stream, + const wpi::math::MecanumDriveWheelAccelerations& value) { wpi_proto_ProtobufMecanumDriveWheelAccelerations msg{ .front_left = value.frontLeft.value(), .front_right = value.frontRight.value(), diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp index 9477e3d3fe9..b8ec92fc577 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp @@ -8,8 +8,8 @@ #include "wpimath/protobuf/kinematics.npb.h" -std::optional -wpi::util::Protobuf::Unpack(InputStream& stream) { +std::optional wpi::util::Protobuf< + wpi::math::SwerveModuleAccelerations>::Unpack(InputStream& stream) { wpi::util::UnpackCallback angle; wpi_proto_ProtobufSwerveModuleAccelerations msg{ .acceleration = 0, diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp index f6deb09e7e0..641ba7beaa1 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp @@ -8,8 +8,8 @@ #include "wpi/math/kinematics/ChassisAccelerations.hpp" -wpi::math::ChassisAccelerations wpi::util::Struct::Unpack( - std::span data) { +wpi::math::ChassisAccelerations wpi::util::Struct< + wpi::math::ChassisAccelerations>::Unpack(std::span data) { constexpr size_t kAxOff = 0; constexpr size_t kAyOff = kAxOff + 8; constexpr size_t kAlphaOff = kAyOff + 8; diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp index 89c65ff7eaf..8c0c05e6df6 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp @@ -28,7 +28,8 @@ wpi::util::Struct::Unpack( } void wpi::util::Struct::Pack( - std::span data, const wpi::math::MecanumDriveWheelAccelerations& value) { + std::span data, + const wpi::math::MecanumDriveWheelAccelerations& value) { constexpr size_t kFrontLeftOff = 0; constexpr size_t kFrontRightOff = kFrontLeftOff + 8; constexpr size_t kRearLeftOff = kFrontRightOff + 8; diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp index 2b9a2e5139b..a5c654eaddc 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp @@ -8,8 +8,9 @@ #include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" -wpi::math::SwerveModuleAccelerations wpi::util::Struct< - wpi::math::SwerveModuleAccelerations>::Unpack(std::span data) { +wpi::math::SwerveModuleAccelerations +wpi::util::Struct::Unpack( + std::span data) { constexpr size_t kAccelerationOff = 0; constexpr size_t kAngleOff = kAccelerationOff + 8; return wpi::math::SwerveModuleAccelerations{ @@ -19,7 +20,8 @@ wpi::math::SwerveModuleAccelerations wpi::util::Struct< } void wpi::util::Struct::Pack( - std::span data, const wpi::math::SwerveModuleAccelerations& value) { + std::span data, + const wpi::math::SwerveModuleAccelerations& value) { constexpr size_t kAccelerationOff = 0; constexpr size_t kAngleOff = kAccelerationOff + 8; wpi::util::PackStruct(data, value.acceleration.value()); diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp index b9df527a05c..331c231d7e3 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp @@ -6,9 +6,9 @@ #include -#include "wpi/math/util/MathUtil.hpp" #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/math/geometry/Translation2d.hpp" +#include "wpi/math/util/MathUtil.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/angular_acceleration.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp index 8eb4a28e91c..bea1f0ca833 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp @@ -475,7 +475,8 @@ class SwerveDriveKinematics * that corner. * @return An array containing the module accelerations. */ - wpi::util::array ToSwerveModuleAccelerations( + wpi::util::array + ToSwerveModuleAccelerations( const ChassisAccelerations& chassisAccelerations, const units::radians_per_second_t angularVelocity = 0.0_rad_per_s, const Translation2d& centerOfRotation = Translation2d{}) const { diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp index dfbd0aeabdb..8657740c398 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp @@ -13,9 +13,12 @@ template <> struct WPILIB_DLLEXPORT wpi::util::Protobuf { using MessageStruct = wpi_proto_ProtobufChassisAccelerations; - using InputStream = wpi::util::ProtoInputStream; - using OutputStream = wpi::util::ProtoOutputStream; - static std::optional Unpack(InputStream& stream); + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); static bool Pack(OutputStream& stream, const wpi::math::ChassisAccelerations& value); }; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp index 2db01115ccd..32cf08d47a4 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp @@ -14,10 +14,10 @@ template <> struct WPILIB_DLLEXPORT wpi::util::Protobuf { using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelAccelerations; - using InputStream = - wpi::util::ProtoInputStream; - using OutputStream = - wpi::util::ProtoOutputStream; + using InputStream = wpi::util::ProtoInputStream< + wpi::math::DifferentialDriveWheelAccelerations>; + using OutputStream = wpi::util::ProtoOutputStream< + wpi::math::DifferentialDriveWheelAccelerations>; static std::optional Unpack( InputStream& stream); static bool Pack(OutputStream& stream, diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp index a21403e0c24..e371d18d902 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp @@ -11,7 +11,8 @@ #include "wpimath/protobuf/kinematics.npb.h" template <> -struct WPILIB_DLLEXPORT wpi::util::Protobuf { +struct WPILIB_DLLEXPORT + wpi::util::Protobuf { using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelAccelerations; using InputStream = wpi::util::ProtoInputStream; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp index 39e1f9b61a2..a934e91fb13 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp @@ -11,10 +11,13 @@ #include "wpimath/protobuf/kinematics.npb.h" template <> -struct WPILIB_DLLEXPORT wpi::util::Protobuf { +struct WPILIB_DLLEXPORT + wpi::util::Protobuf { using MessageStruct = wpi_proto_ProtobufSwerveModuleAccelerations; - using InputStream = wpi::util::ProtoInputStream; - using OutputStream = wpi::util::ProtoOutputStream; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; static std::optional Unpack( InputStream& stream); static bool Pack(OutputStream& stream, diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp index 78538bfc8e1..f212749103e 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp @@ -10,7 +10,8 @@ #include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" template <> -struct WPILIB_DLLEXPORT wpi::util::Struct { +struct WPILIB_DLLEXPORT + wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "DifferentialDriveWheelAccelerations"; } @@ -25,5 +26,5 @@ struct WPILIB_DLLEXPORT wpi::util::Struct); +static_assert(wpi::util::StructSerializable< + wpi::math::DifferentialDriveWheelAccelerations>); diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp index 7b4e18fc1ea..20a65cf7b6d 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp @@ -10,7 +10,8 @@ #include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" template <> -struct WPILIB_DLLEXPORT wpi::util::Struct { +struct WPILIB_DLLEXPORT + wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "MecanumDriveWheelAccelerations"; } @@ -26,4 +27,5 @@ struct WPILIB_DLLEXPORT wpi::util::Struct); +static_assert( + wpi::util::StructSerializable); diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp index 7f899ef88e1..b64b5e6e2ce 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp @@ -10,7 +10,8 @@ #include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" template <> -struct WPILIB_DLLEXPORT wpi::util::Struct { +struct WPILIB_DLLEXPORT + wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "SwerveModuleAccelerations"; } @@ -21,7 +22,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct return "double acceleration;Rotation2d angle"; } - static wpi::math::SwerveModuleAccelerations Unpack(std::span data); + static wpi::math::SwerveModuleAccelerations Unpack( + std::span data); static void Pack(std::span data, const wpi::math::SwerveModuleAccelerations& value); static void ForEachNested( @@ -30,5 +32,6 @@ struct WPILIB_DLLEXPORT wpi::util::Struct } }; -static_assert(wpi::util::StructSerializable); +static_assert( + wpi::util::StructSerializable); static_assert(wpi::util::HasNestedStruct); diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp index 542f7d836b6..28ea4444c5e 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp @@ -8,8 +8,8 @@ #include -#include "wpi/math/kinematics/ChassisSpeeds.hpp" #include "wpi/math/kinematics/ChassisAccelerations.hpp" +#include "wpi/math/kinematics/ChassisSpeeds.hpp" #include "wpi/units/angular_velocity.hpp" #include "wpi/units/length.hpp" #include "wpi/units/velocity.hpp" diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index 4537258ac76..a9278835422 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -315,7 +315,8 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) { ChassisAccelerations accelerations{ 0_mps_sq, 0_mps_sq, wpi::units::radians_per_second_squared_t{2 * std::numbers::pi}}; - wpi::units::radians_per_second_t angularVelocity = 2_rad_per_s * std::numbers::pi; + wpi::units::radians_per_second_t angularVelocity = + 2_rad_per_s * std::numbers::pi; auto [flAccel, frAccel, blAccel, brAccel] = m_kinematics.ToSwerveModuleAccelerations(accelerations, angularVelocity); From d9b2f1e41d10373b6ddbed1026a5e5e885e4553d Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 16:39:59 -0500 Subject: [PATCH 69/82] fuck poseestimator in particular Signed-off-by: Zach Harel --- .../main/native/include/wpi/math/estimator/PoseEstimator.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp index e893c03c1f4..a256344c88a 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp @@ -9,8 +9,6 @@ #include #include -#include -#include #include "wpi/math/geometry/Pose2d.hpp" #include "wpi/math/geometry/Rotation2d.hpp" From 6e687b6ead481f8e9a84b87645f8d7cc3c3a4bc0 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 16:50:45 -0500 Subject: [PATCH 70/82] fixy part 4: javaformat Signed-off-by: Zach Harel --- .../wpilib/math/kinematics/ChassisAccelerations.java | 4 ++-- .../org/wpilib/math/kinematics/ChassisSpeeds.java | 12 +----------- .../math/kinematics/SwerveModuleAccelerations.java | 2 +- .../wpilib/math/kinematics/SwerveModuleState.java | 2 +- .../struct/ChassisAccelerationsStruct.java | 2 +- .../DifferentialDriveWheelAccelerationsStruct.java | 2 +- .../struct/MecanumDriveWheelAccelerationsStruct.java | 2 +- .../struct/SwerveModuleAccelerationsStruct.java | 2 +- .../math/kinematics/ChassisAccelerationsTest.java | 6 +++--- .../DifferentialDriveWheelAccelerationsTest.java | 2 +- .../MecanumDriveWheelAccelerationsTest.java | 2 +- .../math/kinematics/SwerveDriveKinematicsTest.java | 1 + .../kinematics/SwerveModuleAccelerationsTest.java | 4 ++-- 13 files changed, 17 insertions(+), 26 deletions(-) diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java index 50ea56a2a7c..ad68f540672 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java @@ -7,17 +7,17 @@ import static org.wpilib.units.Units.MetersPerSecondPerSecond; import static org.wpilib.units.Units.RadiansPerSecondPerSecond; -import org.wpilib.math.util.MathUtil; +import java.util.Objects; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.interpolation.Interpolatable; import org.wpilib.math.kinematics.proto.ChassisAccelerationsProto; import org.wpilib.math.kinematics.struct.ChassisAccelerationsStruct; +import org.wpilib.math.util.MathUtil; import org.wpilib.units.measure.AngularAcceleration; import org.wpilib.units.measure.LinearAcceleration; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; -import java.util.Objects; /** * Represents the acceleration of a robot chassis. diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java index a1e038972a5..e84f99ce357 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java @@ -7,6 +7,7 @@ import static org.wpilib.units.Units.MetersPerSecond; import static org.wpilib.units.Units.RadiansPerSecond; +import java.util.Objects; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Transform2d; import org.wpilib.math.geometry.Translation2d; @@ -19,17 +20,6 @@ import org.wpilib.units.measure.LinearVelocity; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; -import java.util.Objects; -import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.geometry.Transform2d; -import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.geometry.Twist2d; -import org.wpilib.math.kinematics.proto.ChassisSpeedsProto; -import org.wpilib.math.kinematics.struct.ChassisSpeedsStruct; -import org.wpilib.units.measure.AngularVelocity; -import org.wpilib.units.measure.LinearVelocity; -import org.wpilib.util.protobuf.ProtobufSerializable; -import org.wpilib.util.struct.StructSerializable; /** * Represents the speed of a robot chassis. Although this class contains similar members compared to diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAccelerations.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAccelerations.java index 724c80e62f4..6201942911f 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAccelerations.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAccelerations.java @@ -6,6 +6,7 @@ import static org.wpilib.units.Units.MetersPerSecondPerSecond; +import java.util.Objects; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.interpolation.Interpolatable; import org.wpilib.math.kinematics.proto.SwerveModuleAccelerationsProto; @@ -13,7 +14,6 @@ import org.wpilib.units.measure.LinearAcceleration; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; -import java.util.Objects; /** Represents the accelerations of one swerve module. */ public class SwerveModuleAccelerations diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleState.java index 2e706e9e66d..30335f3d56e 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleState.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleState.java @@ -6,6 +6,7 @@ import static org.wpilib.units.Units.MetersPerSecond; +import java.util.Objects; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.interpolation.Interpolatable; import org.wpilib.math.kinematics.proto.SwerveModuleStateProto; @@ -13,7 +14,6 @@ import org.wpilib.units.measure.LinearVelocity; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; -import java.util.Objects; /** Represents the state of one swerve module. */ public class SwerveModuleState diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/ChassisAccelerationsStruct.java b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/ChassisAccelerationsStruct.java index 974190ce84d..0c97139a0a6 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/ChassisAccelerationsStruct.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/ChassisAccelerationsStruct.java @@ -4,9 +4,9 @@ package org.wpilib.math.kinematics.struct; +import java.nio.ByteBuffer; import org.wpilib.math.kinematics.ChassisAccelerations; import org.wpilib.util.struct.Struct; -import java.nio.ByteBuffer; public class ChassisAccelerationsStruct implements Struct { @Override diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java index 0b4349d018f..8441901ac61 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java @@ -4,9 +4,9 @@ package org.wpilib.math.kinematics.struct; +import java.nio.ByteBuffer; import org.wpilib.math.kinematics.DifferentialDriveWheelAccelerations; import org.wpilib.util.struct.Struct; -import java.nio.ByteBuffer; public class DifferentialDriveWheelAccelerationsStruct implements Struct { diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java index f3e3287aab8..38f9777d5f3 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java @@ -4,9 +4,9 @@ package org.wpilib.math.kinematics.struct; +import java.nio.ByteBuffer; import org.wpilib.math.kinematics.MecanumDriveWheelAccelerations; import org.wpilib.util.struct.Struct; -import java.nio.ByteBuffer; public class MecanumDriveWheelAccelerationsStruct implements Struct { diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationsStruct.java b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationsStruct.java index cbb89adeeef..08e7fa89823 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationsStruct.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationsStruct.java @@ -4,10 +4,10 @@ package org.wpilib.math.kinematics.struct; +import java.nio.ByteBuffer; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModuleAccelerations; import org.wpilib.util.struct.Struct; -import java.nio.ByteBuffer; public class SwerveModuleAccelerationsStruct implements Struct { @Override diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/ChassisAccelerationsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/ChassisAccelerationsTest.java index 2624420cbef..863ca72c821 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/ChassisAccelerationsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/ChassisAccelerationsTest.java @@ -4,13 +4,13 @@ package org.wpilib.math.kinematics; -import static org.wpilib.units.Units.MetersPerSecondPerSecond; -import static org.wpilib.units.Units.RadiansPerSecondPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.wpilib.units.Units.MetersPerSecondPerSecond; +import static org.wpilib.units.Units.RadiansPerSecondPerSecond; -import org.wpilib.math.geometry.Rotation2d; import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation2d; class ChassisAccelerationsTest { private static final double kEpsilon = 1E-9; diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerationsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerationsTest.java index 3fa1b2869d7..38022f2ebfd 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerationsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerationsTest.java @@ -4,9 +4,9 @@ package org.wpilib.math.kinematics; -import static org.wpilib.units.Units.MetersPerSecondPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.wpilib.units.Units.MetersPerSecondPerSecond; import org.junit.jupiter.api.Test; diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelAccelerationsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelAccelerationsTest.java index 37373ccf0d5..328e310a4f2 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelAccelerationsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelAccelerationsTest.java @@ -4,9 +4,9 @@ package org.wpilib.math.kinematics; -import static org.wpilib.units.Units.MetersPerSecondPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.wpilib.units.Units.MetersPerSecondPerSecond; import org.junit.jupiter.api.Test; diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java index eccd3122214..9b05c660d83 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java @@ -7,6 +7,7 @@ import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import java.util.Arrays; import org.junit.jupiter.api.Test; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationsTest.java index bd98211cc11..19e7f10216f 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationsTest.java @@ -4,14 +4,14 @@ package org.wpilib.math.kinematics; -import static org.wpilib.units.Units.MetersPerSecondPerSecond; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.wpilib.units.Units.MetersPerSecondPerSecond; -import org.wpilib.math.geometry.Rotation2d; import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation2d; class SwerveModuleAccelerationsTest { private static final double kEpsilon = 1E-9; From 43d8bb50dac9e4f0cc67293c299a1b3149e88aa5 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 17:06:19 -0500 Subject: [PATCH 71/82] fixy part 5: includes use double quotes instead of angle brackets Signed-off-by: Zach Harel --- .../cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp | 2 +- .../cpp/kinematics/struct/ChassisAccelerationsStruct.cpp | 2 +- .../struct/DifferentialDriveWheelAccelerationsStruct.cpp | 2 +- .../struct/MecanumDriveWheelAccelerationsStruct.cpp | 2 +- .../cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp | 2 +- .../include/wpi/math/kinematics/ChassisAccelerations.hpp | 2 +- .../math/kinematics/DifferentialDriveWheelAccelerations.hpp | 2 +- .../wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp | 2 +- .../include/wpi/math/kinematics/SwerveModuleAccelerations.hpp | 2 +- .../wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp | 4 ++-- .../proto/DifferentialDriveWheelAccelerationsProto.hpp | 4 ++-- .../kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp | 4 ++-- .../math/kinematics/proto/SwerveModuleAccelerationsProto.hpp | 4 ++-- .../wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp | 4 ++-- .../struct/DifferentialDriveWheelAccelerationsStruct.hpp | 4 ++-- .../struct/MecanumDriveWheelAccelerationsStruct.hpp | 4 ++-- .../kinematics/struct/SwerveModuleAccelerationsStruct.hpp | 4 ++-- 17 files changed, 25 insertions(+), 25 deletions(-) diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp index b8ec92fc577..8ca2f151306 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp" -#include +#include "wpi/util/protobuf/ProtobufCallbacks.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp index 641ba7beaa1..81b310cfb3b 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp" -#include +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/ChassisAccelerations.hpp" diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp index 215e32c49a8..8dd61e17400 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp" -#include +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp index 8c0c05e6df6..d41c6334fa3 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp" -#include +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp index a5c654eaddc..a9548a827e1 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp" -#include +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp index 331c231d7e3..9c7944e3ee4 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp @@ -4,7 +4,7 @@ #pragma once -#include +#include "wpi/util/SymbolExports.hpp" #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/math/geometry/Translation2d.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp index edd96bb97ff..12a939fb314 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp @@ -4,7 +4,7 @@ #pragma once -#include +#include "wpi/util/SymbolExports.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp index d26ef1e1dff..af2235aba20 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp @@ -8,7 +8,7 @@ #include #include -#include +#include "wpi/util/SymbolExports.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp index c37bc2a7b85..fb6c12d468b 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp @@ -4,7 +4,7 @@ #pragma once -#include +#include "wpi/util/SymbolExports.hpp" #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/units/acceleration.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp index 8657740c398..1edb80d0a18 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" #include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp index 32cf08d47a4..37d74fd121e 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" #include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp index e371d18d902..271b9e9fb95 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" #include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp index a934e91fb13..16e1974f6a2 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" #include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp index 1ae4d85c618..4f0c3134d88 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/ChassisAccelerations.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp index f212749103e..a4fb408e26f 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp index 20a65cf7b6d..7d204a4e634 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp index b64b5e6e2ce..5880cb50248 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" From 1213b3dde78e53e51292ce9e427aba721fad0b66 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 17:06:19 -0500 Subject: [PATCH 72/82] fixy part 5: includes use double quotes instead of angle brackets Signed-off-by: Zach Harel --- .../cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp | 2 +- .../cpp/kinematics/struct/ChassisAccelerationsStruct.cpp | 2 +- .../struct/DifferentialDriveWheelAccelerationsStruct.cpp | 2 +- .../struct/MecanumDriveWheelAccelerationsStruct.cpp | 2 +- .../cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp | 2 +- .../include/wpi/math/kinematics/ChassisAccelerations.hpp | 2 +- .../math/kinematics/DifferentialDriveWheelAccelerations.hpp | 2 +- .../wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp | 2 +- .../include/wpi/math/kinematics/SwerveModuleAccelerations.hpp | 2 +- .../wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp | 4 ++-- .../proto/DifferentialDriveWheelAccelerationsProto.hpp | 4 ++-- .../kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp | 4 ++-- .../math/kinematics/proto/SwerveModuleAccelerationsProto.hpp | 4 ++-- .../wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp | 4 ++-- .../struct/DifferentialDriveWheelAccelerationsStruct.hpp | 4 ++-- .../struct/MecanumDriveWheelAccelerationsStruct.hpp | 4 ++-- .../kinematics/struct/SwerveModuleAccelerationsStruct.hpp | 4 ++-- 17 files changed, 25 insertions(+), 25 deletions(-) diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp index b8ec92fc577..8ca2f151306 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp" -#include +#include "wpi/util/protobuf/ProtobufCallbacks.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp index 641ba7beaa1..81b310cfb3b 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp" -#include +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/ChassisAccelerations.hpp" diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp index 215e32c49a8..8dd61e17400 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp" -#include +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp index 8c0c05e6df6..d41c6334fa3 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp" -#include +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp index a5c654eaddc..a9548a827e1 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp @@ -4,7 +4,7 @@ #include "wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp" -#include +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp index 331c231d7e3..9c7944e3ee4 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp @@ -4,7 +4,7 @@ #pragma once -#include +#include "wpi/util/SymbolExports.hpp" #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/math/geometry/Translation2d.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp index edd96bb97ff..12a939fb314 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp @@ -4,7 +4,7 @@ #pragma once -#include +#include "wpi/util/SymbolExports.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp index d26ef1e1dff..af2235aba20 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp @@ -8,7 +8,7 @@ #include #include -#include +#include "wpi/util/SymbolExports.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp index c37bc2a7b85..fb6c12d468b 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp @@ -4,7 +4,7 @@ #pragma once -#include +#include "wpi/util/SymbolExports.hpp" #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/units/acceleration.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp index 8657740c398..1edb80d0a18 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" #include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp index 32cf08d47a4..37d74fd121e 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" #include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp index e371d18d902..271b9e9fb95 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" #include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp index a934e91fb13..16e1974f6a2 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/protobuf/Protobuf.hpp" #include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp index 1ae4d85c618..4f0c3134d88 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/ChassisAccelerations.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp index f212749103e..a4fb408e26f 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp index 20a65cf7b6d..7d204a4e634 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp index b64b5e6e2ce..5880cb50248 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp @@ -4,8 +4,8 @@ #pragma once -#include -#include +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" #include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" From 8a6f519888c1dda11a90362755ac66e0399aa929 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 13 Nov 2025 17:10:18 -0500 Subject: [PATCH 73/82] fixy part 6: run wpiformat after fixy part 5 Signed-off-by: Zach Harel --- .../cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp | 1 - .../cpp/kinematics/struct/ChassisAccelerationsStruct.cpp | 3 +-- .../struct/DifferentialDriveWheelAccelerationsStruct.cpp | 3 +-- .../kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp | 3 +-- .../cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp | 3 +-- .../include/wpi/math/kinematics/ChassisAccelerations.hpp | 3 +-- .../math/kinematics/DifferentialDriveWheelAccelerations.hpp | 3 +-- .../wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp | 3 +-- .../include/wpi/math/kinematics/SwerveModuleAccelerations.hpp | 3 +-- .../wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp | 3 +-- .../proto/DifferentialDriveWheelAccelerationsProto.hpp | 3 +-- .../kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp | 3 +-- .../math/kinematics/proto/SwerveModuleAccelerationsProto.hpp | 3 +-- .../wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp | 3 +-- .../struct/DifferentialDriveWheelAccelerationsStruct.hpp | 3 +-- .../kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp | 3 +-- .../math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp | 3 +-- 17 files changed, 16 insertions(+), 33 deletions(-) diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp index 8ca2f151306..197f0b1e99a 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp @@ -5,7 +5,6 @@ #include "wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp" #include "wpi/util/protobuf/ProtobufCallbacks.hpp" - #include "wpimath/protobuf/kinematics.npb.h" std::optional wpi::util::Protobuf< diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp index 81b310cfb3b..1a5113b0c6e 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.cpp @@ -4,9 +4,8 @@ #include "wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp" -#include "wpi/util/struct/Struct.hpp" - #include "wpi/math/kinematics/ChassisAccelerations.hpp" +#include "wpi/util/struct/Struct.hpp" wpi::math::ChassisAccelerations wpi::util::Struct< wpi::math::ChassisAccelerations>::Unpack(std::span data) { diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp index 8dd61e17400..90918883025 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp @@ -4,9 +4,8 @@ #include "wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp" -#include "wpi/util/struct/Struct.hpp" - #include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" +#include "wpi/util/struct/Struct.hpp" wpi::math::DifferentialDriveWheelAccelerations wpi::util::Struct::Unpack( diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp index d41c6334fa3..2fcd821b1a4 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp @@ -4,9 +4,8 @@ #include "wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp" -#include "wpi/util/struct/Struct.hpp" - #include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" +#include "wpi/util/struct/Struct.hpp" wpi::math::MecanumDriveWheelAccelerations wpi::util::Struct::Unpack( diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp index a9548a827e1..d9d43156847 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp @@ -4,9 +4,8 @@ #include "wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp" -#include "wpi/util/struct/Struct.hpp" - #include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" +#include "wpi/util/struct/Struct.hpp" wpi::math::SwerveModuleAccelerations wpi::util::Struct::Unpack( diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp index 9c7944e3ee4..f97bf0f295d 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp @@ -4,13 +4,12 @@ #pragma once -#include "wpi/util/SymbolExports.hpp" - #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/math/geometry/Translation2d.hpp" #include "wpi/math/util/MathUtil.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/angular_acceleration.hpp" +#include "wpi/util/SymbolExports.hpp" namespace wpi::math { /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp index 12a939fb314..932184a6987 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp @@ -4,10 +4,9 @@ #pragma once -#include "wpi/util/SymbolExports.hpp" - #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" +#include "wpi/util/SymbolExports.hpp" namespace wpi::math { /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp index af2235aba20..713e3119954 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp @@ -8,10 +8,9 @@ #include #include -#include "wpi/util/SymbolExports.hpp" - #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" +#include "wpi/util/SymbolExports.hpp" namespace wpi::math { /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp index fb6c12d468b..3fef5b8f11d 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp @@ -4,11 +4,10 @@ #pragma once -#include "wpi/util/SymbolExports.hpp" - #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/math.hpp" +#include "wpi/util/SymbolExports.hpp" namespace wpi::math { /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp index 1edb80d0a18..7ab7485db20 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp @@ -4,10 +4,9 @@ #pragma once +#include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/protobuf/Protobuf.hpp" - -#include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" template <> diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp index 37d74fd121e..8fb7aca097d 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp @@ -4,10 +4,9 @@ #pragma once +#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/protobuf/Protobuf.hpp" - -#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" template <> diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp index 271b9e9fb95..2adaff0498d 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp @@ -4,10 +4,9 @@ #pragma once +#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/protobuf/Protobuf.hpp" - -#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" template <> diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp index 16e1974f6a2..d57185a97b1 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp @@ -4,10 +4,9 @@ #pragma once +#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/protobuf/Protobuf.hpp" - -#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" #include "wpimath/protobuf/kinematics.npb.h" template <> diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp index 4f0c3134d88..1279c2e0178 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp @@ -4,11 +4,10 @@ #pragma once +#include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/struct/Struct.hpp" -#include "wpi/math/kinematics/ChassisAccelerations.hpp" - template <> struct WPILIB_DLLEXPORT wpi::util::Struct { static constexpr std::string_view GetTypeName() { diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp index a4fb408e26f..3e32f90813e 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp @@ -4,11 +4,10 @@ #pragma once +#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/struct/Struct.hpp" -#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" - template <> struct WPILIB_DLLEXPORT wpi::util::Struct { diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp index 7d204a4e634..6265989687c 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp @@ -4,11 +4,10 @@ #pragma once +#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/struct/Struct.hpp" -#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" - template <> struct WPILIB_DLLEXPORT wpi::util::Struct { diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp index 5880cb50248..db6277abb38 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp @@ -4,11 +4,10 @@ #pragma once +#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/struct/Struct.hpp" -#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" - template <> struct WPILIB_DLLEXPORT wpi::util::Struct { From a4fc349d72f1642e787580351ef924a32acdf60d Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 14 Nov 2025 01:43:04 -0500 Subject: [PATCH 74/82] Add python bindings Signed-off-by: Zach Harel --- wpimath/robotpy_pybind_build_info.bzl | 40 +++++++++++++++++++ wpimath/src/main/python/pyproject.toml | 4 ++ .../kinematics/ChassisAccelerations.yml | 17 ++++++++ .../DifferentialDriveKinematics.yml | 2 + .../DifferentialDriveWheelAccelerations.yml | 14 +++++++ .../python/semiwrap/kinematics/Kinematics.yml | 2 + .../kinematics/MecanumDriveKinematics.yml | 5 +++ .../MecanumDriveWheelAccelerations.yml | 16 ++++++++ .../kinematics/SwerveDriveKinematics.yml | 6 +++ .../kinematics/SwerveModuleAccelerations.yml | 14 +++++++ 10 files changed, 120 insertions(+) create mode 100644 wpimath/src/main/python/semiwrap/kinematics/ChassisAccelerations.yml create mode 100644 wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveWheelAccelerations.yml create mode 100644 wpimath/src/main/python/semiwrap/kinematics/MecanumDriveWheelAccelerations.yml create mode 100644 wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAccelerations.yml diff --git a/wpimath/robotpy_pybind_build_info.bzl b/wpimath/robotpy_pybind_build_info.bzl index e6e21ae0003..691814a2cae 100644 --- a/wpimath/robotpy_pybind_build_info.bzl +++ b/wpimath/robotpy_pybind_build_info.bzl @@ -576,6 +576,16 @@ def wpimath_kinematics_extension(srcs = [], header_to_dat_deps = [], extra_hdrs ("wpi::math::ChassisSpeeds", "wpi__math__ChassisSpeeds.hpp"), ], ), + struct( + class_name = "ChassisAccelerations", + yml_file = "semiwrap/kinematics/ChassisAccelerations.yml", + header_root = "$(execpath :robotpy-native-wpimath.copy_headers)", + header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/ChassisAccelerations.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::math::ChassisAccelerations", "wpi__math__ChassisAccelerations.hpp"), + ], + ), struct( class_name = "DifferentialDriveKinematics", yml_file = "semiwrap/kinematics/DifferentialDriveKinematics.yml", @@ -626,6 +636,16 @@ def wpimath_kinematics_extension(srcs = [], header_to_dat_deps = [], extra_hdrs ("wpi::math::DifferentialDriveWheelSpeeds", "wpi__math__DifferentialDriveWheelSpeeds.hpp"), ], ), + struct( + class_name = "DifferentialDriveWheelAccelerations", + yml_file = "semiwrap/kinematics/DifferentialDriveWheelAccelerations.yml", + header_root = "$(execpath :robotpy-native-wpimath.copy_headers)", + header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::math::DifferentialDriveWheelAccelerations", "wpi__math__DifferentialDriveWheelAccelerations.hpp"), + ], + ), struct( class_name = "Kinematics", yml_file = "semiwrap/kinematics/Kinematics.yml", @@ -693,6 +713,16 @@ def wpimath_kinematics_extension(srcs = [], header_to_dat_deps = [], extra_hdrs ("wpi::math::MecanumDriveWheelSpeeds", "wpi__math__MecanumDriveWheelSpeeds.hpp"), ], ), + struct( + class_name = "MecanumDriveWheelAccelerations", + yml_file = "semiwrap/kinematics/MecanumDriveWheelAccelerations.yml", + header_root = "$(execpath :robotpy-native-wpimath.copy_headers)", + header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::math::MecanumDriveWheelAccelerations", "wpi__math__MecanumDriveWheelAccelerations.hpp"), + ], + ), struct( class_name = "Odometry", yml_file = "semiwrap/kinematics/Odometry.yml", @@ -792,6 +822,16 @@ def wpimath_kinematics_extension(srcs = [], header_to_dat_deps = [], extra_hdrs ("wpi::math::SwerveModuleState", "wpi__math__SwerveModuleState.hpp"), ], ), + struct( + class_name = "SwerveModuleAccelerations", + yml_file = "semiwrap/kinematics/SwerveModuleAccelerations.yml", + header_root = "$(execpath :robotpy-native-wpimath.copy_headers)", + header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/SwerveModuleAccelerations.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::math::SwerveModuleAccelerations", "wpi__math__SwerveModuleAccelerations.hpp"), + ], + ), ] resolve_casters( diff --git a/wpimath/src/main/python/pyproject.toml b/wpimath/src/main/python/pyproject.toml index 3a36a798a08..82351f9c656 100644 --- a/wpimath/src/main/python/pyproject.toml +++ b/wpimath/src/main/python/pyproject.toml @@ -1526,17 +1526,20 @@ yaml_path = "semiwrap/kinematics" [tool.semiwrap.extension_modules."wpimath.kinematics._kinematics".headers] # wpi/math/kinematics ChassisSpeeds = "wpi/math/kinematics/ChassisSpeeds.hpp" +ChassisAccelerations = "wpi/math/kinematics/ChassisAccelerations.hpp" DifferentialDriveKinematics = "wpi/math/kinematics/DifferentialDriveKinematics.hpp" DifferentialDriveOdometry3d = "wpi/math/kinematics/DifferentialDriveOdometry3d.hpp" DifferentialDriveOdometry = "wpi/math/kinematics/DifferentialDriveOdometry.hpp" DifferentialDriveWheelPositions = "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp" DifferentialDriveWheelSpeeds = "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp" +DifferentialDriveWheelAccelerations = "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" Kinematics = "wpi/math/kinematics/Kinematics.hpp" MecanumDriveKinematics = "wpi/math/kinematics/MecanumDriveKinematics.hpp" MecanumDriveOdometry = "wpi/math/kinematics/MecanumDriveOdometry.hpp" MecanumDriveOdometry3d = "wpi/math/kinematics/MecanumDriveOdometry3d.hpp" MecanumDriveWheelPositions = "wpi/math/kinematics/MecanumDriveWheelPositions.hpp" MecanumDriveWheelSpeeds = "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp" +MecanumDriveWheelAccelerations = "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" Odometry = "wpi/math/kinematics/Odometry.hpp" Odometry3d = "wpi/math/kinematics/Odometry3d.hpp" SwerveDriveKinematics = "wpi/math/kinematics/SwerveDriveKinematics.hpp" @@ -1544,6 +1547,7 @@ SwerveDriveOdometry = "wpi/math/kinematics/SwerveDriveOdometry.hpp" SwerveDriveOdometry3d = "wpi/math/kinematics/SwerveDriveOdometry3d.hpp" SwerveModulePosition = "wpi/math/kinematics/SwerveModulePosition.hpp" SwerveModuleState = "wpi/math/kinematics/SwerveModuleState.hpp" +SwerveModuleAccelerations = "wpi/math/kinematics/SwerveModuleAccelerations.hpp" [tool.semiwrap.extension_modules."wpimath.spline._spline"] diff --git a/wpimath/src/main/python/semiwrap/kinematics/ChassisAccelerations.yml b/wpimath/src/main/python/semiwrap/kinematics/ChassisAccelerations.yml new file mode 100644 index 00000000000..d6eb353f706 --- /dev/null +++ b/wpimath/src/main/python/semiwrap/kinematics/ChassisAccelerations.yml @@ -0,0 +1,17 @@ +classes: + wpi::math::ChassisAccelerations: + attributes: + ax: + ay: + alpha: + methods: + ToRobotRelative: + ToFieldRelative: + operator+: + operator-: + overloads: + const ChassisAccelerations& [const]: + '[const]': + operator*: + operator/: + operator==: diff --git a/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveKinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveKinematics.yml index 04b2ee336b9..76168dac20a 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveKinematics.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveKinematics.yml @@ -14,6 +14,8 @@ classes: const wpi::units::meter_t, const wpi::units::meter_t [const]: const DifferentialDriveWheelPositions&, const DifferentialDriveWheelPositions& [const]: Interpolate: + ToChassisAccelerations: + ToWheelAccelerations: inline_code: | SetupWPyStruct(cls_DifferentialDriveKinematics); diff --git a/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveWheelAccelerations.yml b/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveWheelAccelerations.yml new file mode 100644 index 00000000000..47c41a4f0b7 --- /dev/null +++ b/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveWheelAccelerations.yml @@ -0,0 +1,14 @@ +classes: + wpi::math::DifferentialDriveWheelAccelerations: + attributes: + left: + right: + methods: + operator+: + operator-: + overloads: + const DifferentialDriveWheelAccelerations& [const]: + '[const]': + operator*: + operator/: + operator==: diff --git a/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml index 57b87c12e2a..70c537614d6 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml @@ -18,6 +18,8 @@ classes: ToWheelSpeeds: ToTwist2d: Interpolate: + ToChassisAccelerations: + ToWheelAccelerations: templates: diff --git a/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveKinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveKinematics.yml index b97bf4ee4e9..219e7d4928e 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveKinematics.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveKinematics.yml @@ -45,6 +45,11 @@ classes: GetRearLeft: GetRearRight: Interpolate: + ToChassisAccelerations: + ToWheelAccelerations: + overloads: + const ChassisAccelerations&, const Translation2d& [const]: + const ChassisAccelerations& [const]: inline_code: | SetupWPyStruct(cls_MecanumDriveKinematics); diff --git a/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveWheelAccelerations.yml b/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveWheelAccelerations.yml new file mode 100644 index 00000000000..f69a33d8b18 --- /dev/null +++ b/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveWheelAccelerations.yml @@ -0,0 +1,16 @@ +classes: + wpi::math::MecanumDriveWheelAccelerations: + attributes: + frontLeft: + frontRight: + rearLeft: + rearRight: + methods: + operator+: + operator-: + overloads: + const MecanumDriveWheelAccelerations& [const]: + '[const]': + operator*: + operator/: + operator==: diff --git a/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml index 141fd96668e..5bb3f39d9df 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml @@ -77,6 +77,12 @@ classes: Interpolate: GetModules: + ToSwerveModuleAccelerations: + ToWheelAccelerations: + ToChassisAccelerations: + overloads: + ModuleAccelerations&&... [const]: + const wpi::util::array& [const]: template_inline_code: | if constexpr (NumModules == 2) { diff --git a/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAccelerations.yml b/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAccelerations.yml new file mode 100644 index 00000000000..296023b519f --- /dev/null +++ b/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAccelerations.yml @@ -0,0 +1,14 @@ +classes: + wpi::math::SwerveModuleAccelerations: + attributes: + acceleration: + angle: + methods: + operator==: + operator+: + operator-: + overloads: + const SwerveModuleAccelerations& [const]: + '[const]': + operator*: + operator/: From 84fb2b226cdb14106fa91c438a9a90d8b422ac10 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sat, 15 Nov 2025 21:38:58 -0500 Subject: [PATCH 75/82] Add support for wheel accelerations in kinematics and odometry robotpy binding configurations Signed-off-by: Zach Harel --- .../main/python/semiwrap/controls/PoseEstimator.yml | 10 ++++++++++ .../main/python/semiwrap/controls/PoseEstimator3d.yml | 10 ++++++++++ .../src/main/python/semiwrap/kinematics/Kinematics.yml | 10 ++++++++++ .../src/main/python/semiwrap/kinematics/Odometry.yml | 10 ++++++++++ .../src/main/python/semiwrap/kinematics/Odometry3d.yml | 10 ++++++++++ .../semiwrap/kinematics/SwerveDriveKinematics.yml | 1 + 6 files changed, 51 insertions(+) diff --git a/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml b/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml index 5e95d1dde32..33f99446963 100644 --- a/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml +++ b/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml @@ -2,10 +2,13 @@ defaults: subpackage: estimator extra_includes: +- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp - wpi/math/kinematics/DifferentialDriveWheelPositions.hpp - wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp +- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp - wpi/math/kinematics/MecanumDriveWheelPositions.hpp - wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp +- wpi/math/kinematics/SwerveModuleAccelerations.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp @@ -14,6 +17,7 @@ classes: template_params: - WheelSpeeds - WheelPositions + - WheelAccelerations methods: PoseEstimator: SetVisionMeasurementStdDevs: @@ -36,28 +40,34 @@ templates: params: - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelAccelerations MecanumDrivePoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive3PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive4PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive6PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - wpi::util::array - wpi::util::array + - wpi::util::array diff --git a/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml b/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml index 69cd62b6e68..40453de0f47 100644 --- a/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml +++ b/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml @@ -2,10 +2,13 @@ defaults: subpackage: estimator extra_includes: +- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp - wpi/math/kinematics/DifferentialDriveWheelPositions.hpp - wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp +- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp - wpi/math/kinematics/MecanumDriveWheelPositions.hpp - wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp +- wpi/math/kinematics/SwerveModuleAccelerations.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp classes: @@ -13,6 +16,7 @@ classes: template_params: - WheelSpeeds - WheelPositions + - WheelAccelerations methods: PoseEstimator3d: SetVisionMeasurementStdDevs: @@ -36,28 +40,34 @@ templates: params: - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelAccelerations MecanumDrivePoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive3PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive4PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive6PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - wpi::util::array - wpi::util::array + - wpi::util::array diff --git a/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml index 70c537614d6..6ce0b74ee19 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml @@ -1,8 +1,11 @@ extra_includes: +- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp - wpi/math/kinematics/DifferentialDriveWheelPositions.hpp - wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp +- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp - wpi/math/kinematics/MecanumDriveWheelPositions.hpp - wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp +- wpi/math/kinematics/SwerveModuleAccelerations.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp @@ -13,6 +16,7 @@ classes: template_params: - WheelSpeeds - WheelPositions + - WheelAccelerations methods: ToChassisSpeeds: ToWheelSpeeds: @@ -28,28 +32,34 @@ templates: params: - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelAccelerations MecanumDriveKinematicsBase: qualname: wpi::math::Kinematics params: - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2KinematicsBase: qualname: wpi::math::Kinematics params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive3KinematicsBase: qualname: wpi::math::Kinematics params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive4KinematicsBase: qualname: wpi::math::Kinematics params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive6KinematicsBase: qualname: wpi::math::Kinematics params: - wpi::util::array - wpi::util::array + - wpi::util::array diff --git a/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml b/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml index d144ac62add..d3832285357 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml @@ -1,8 +1,11 @@ extra_includes: +- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp - wpi/math/kinematics/DifferentialDriveWheelPositions.hpp - wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp +- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp - wpi/math/kinematics/MecanumDriveWheelPositions.hpp - wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp +- wpi/math/kinematics/SwerveModuleAccelerations.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp classes: @@ -10,6 +13,7 @@ classes: template_params: - WheelSpeeds - WheelPositions + - WheelAccelerations methods: Odometry: ResetPosition: @@ -25,28 +29,34 @@ templates: params: - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelAccelerations MecanumDriveOdometryBase: qualname: wpi::math::Odometry params: - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2OdometryBase: qualname: wpi::math::Odometry params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive3OdometryBase: qualname: wpi::math::Odometry params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive4OdometryBase: qualname: wpi::math::Odometry params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive6OdometryBase: qualname: wpi::math::Odometry params: - wpi::util::array - wpi::util::array + - wpi::util::array diff --git a/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml b/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml index 6b1657f5f41..4bfa19236a6 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml @@ -1,8 +1,11 @@ extra_includes: +- wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp - wpi/math/kinematics/DifferentialDriveWheelPositions.hpp - wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp +- wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp - wpi/math/kinematics/MecanumDriveWheelPositions.hpp - wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp +- wpi/math/kinematics/SwerveModuleAccelerations.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp classes: @@ -10,6 +13,7 @@ classes: template_params: - WheelSpeeds - WheelPositions + - WheelAccelerations methods: Odometry3d: ResetPosition: @@ -26,28 +30,34 @@ templates: params: - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelAccelerations MecanumDriveOdometry3dBase: qualname: wpi::math::Odometry3d params: - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2Odometry3dBase: qualname: wpi::math::Odometry3d params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive3Odometry3dBase: qualname: wpi::math::Odometry3d params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive4Odometry3dBase: qualname: wpi::math::Odometry3d params: - wpi::util::array - wpi::util::array + - wpi::util::array SwerveDrive6Odometry3dBase: qualname: wpi::math::Odometry3d params: - wpi::util::array - wpi::util::array + - wpi::util::array diff --git a/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml index 5bb3f39d9df..d1c36cd5e09 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml @@ -82,6 +82,7 @@ classes: ToChassisAccelerations: overloads: ModuleAccelerations&&... [const]: + ignore: true const wpi::util::array& [const]: template_inline_code: | From f9ef343fe8699c196f6e7cf58a1332bc5d947655 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sun, 16 Nov 2025 20:17:54 -0500 Subject: [PATCH 76/82] fix kinematics protobuf generation Signed-off-by: Zach Harel --- .../org/wpilib/math/proto/Kinematics.java | 136 ++--- .../cpp/wpimath/protobuf/kinematics.npb.cpp | 486 +++++++++--------- 2 files changed, 311 insertions(+), 311 deletions(-) diff --git a/wpimath/src/generated/main/java/org/wpilib/math/proto/Kinematics.java b/wpimath/src/generated/main/java/org/wpilib/math/proto/Kinematics.java index 5886f5b7495..bd330165a9b 100644 --- a/wpimath/src/generated/main/java/org/wpilib/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/org/wpilib/math/proto/Kinematics.java @@ -19,77 +19,77 @@ import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4189, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4186, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + - "BW9tZWdhIlQKHFByb3RvYnVmQ2hhc3Npc0FjY2VsZXJhdGlvbnMSDgoCYXgYASABKAFSAmF4Eg4KAmF5" + - "GAIgASgBUgJheRIUCgVhbHBoYRgDIAEoAVIFYWxwaGEiRQojUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2" + - "ZUtpbmVtYXRpY3MSHgoKdHJhY2t3aWR0aBgBIAEoAVIKdHJhY2t3aWR0aCJQCiRQcm90b2J1ZkRpZmZl" + - "cmVudGlhbERyaXZlV2hlZWxTcGVlZHMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIF" + - "cmlnaHQiVworUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsQWNjZWxlcmF0aW9ucxISCgRsZWZ0" + - "GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCJTCidQcm90b2J1ZkRpZmZlcmVudGlhbERy" + - "aXZlV2hlZWxQb3NpdGlvbnMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIFcmlnaHQi" + - "pAIKHlByb3RvYnVmTWVjYW51bURyaXZlS2luZW1hdGljcxI/Cgpmcm9udF9sZWZ0GAEgASgLMiAud3Bp" + - "LnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJZnJvbnRMZWZ0EkEKC2Zyb250X3JpZ2h0GAIgASgL" + - "MiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIKZnJvbnRSaWdodBI9CglyZWFyX2xlZnQY" + - "AyABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUghyZWFyTGVmdBI/CgpyZWFyX3Jp" + - "Z2h0GAQgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJcmVhclJpZ2h0IqABCiJQ" + - "cm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250" + - "TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJl" + - "YXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNhbnVtRHJp" + - "dmVXaGVlbFNwZWVkcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQY" + - "AiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0" + + "BW9tZWdhIlQKHFByb3RvYnVmQ2hhc3Npc0FjY2VsZXJhdGlvbnMSDgoCYXgYASABKAFSAmF4Eg4KAmF5" + + "GAIgASgBUgJheRIUCgVhbHBoYRgDIAEoAVIFYWxwaGEiRQojUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2" + + "ZUtpbmVtYXRpY3MSHgoKdHJhY2t3aWR0aBgBIAEoAVIKdHJhY2t3aWR0aCJQCiRQcm90b2J1ZkRpZmZl" + + "cmVudGlhbERyaXZlV2hlZWxTcGVlZHMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIF" + + "cmlnaHQiVworUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsQWNjZWxlcmF0aW9ucxISCgRsZWZ0" + + "GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCJTCidQcm90b2J1ZkRpZmZlcmVudGlhbERy" + + "aXZlV2hlZWxQb3NpdGlvbnMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIFcmlnaHQi" + + "pAIKHlByb3RvYnVmTWVjYW51bURyaXZlS2luZW1hdGljcxI/Cgpmcm9udF9sZWZ0GAEgASgLMiAud3Bp" + + "LnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJZnJvbnRMZWZ0EkEKC2Zyb250X3JpZ2h0GAIgASgL" + + "MiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIKZnJvbnRSaWdodBI9CglyZWFyX2xlZnQY" + + "AyABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUghyZWFyTGVmdBI/CgpyZWFyX3Jp" + + "Z2h0GAQgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJcmVhclJpZ2h0IqABCiJQ" + + "cm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250" + + "TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJl" + + "YXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNhbnVtRHJp" + + "dmVXaGVlbFNwZWVkcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQY" + + "AiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0" + "GAQgASgBUglyZWFyUmlnaHQipAEKJlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxBY2NlbGVyYXRpb25z", - "Eh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRS" + - "aWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJS" + - "aWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURyaXZlS2luZW1hdGljcxI6Cgdtb2R1bGVzGAEgAygLMiAud3Bp" + - "LnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIHbW9kdWxlcyJvChxQcm90b2J1ZlN3ZXJ2ZU1vZHVs" + - "ZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5w" + - "cm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUS" + - "FAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90" + - "YXRpb24yZFIFYW5nbGUifAohUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb25zEiIKDGFjY2Vs" + - "ZXJhdGlvbhgBIAEoAVIMYWNjZWxlcmF0aW9uEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3Rv" + - "YnVmUm90YXRpb24yZFIFYW5nbGVCGgoYZWR1LndwaS5maXJzdC5tYXRoLnByb3RvStYSCgYSBAAAVAEK" + - "CAoBDBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAaCggKAQgSAwYAMQoJCgIIARIDBgAxCgoKAgQAEgQI" + - "AAwBCgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoMCgUEAAIABRIDCQIICgwKBQQAAgABEgMJCQsKDAoF" + - "BAACAAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAACAQUSAwoCCAoMCgUEAAIBARIDCgkLCgwKBQQAAgED" + - "EgMKDg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgMLAggKDAoFBAACAgESAwsJDgoMCgUEAAICAxIDCxES" + - "CgoKAgQBEgQOABIBCgoKAwQBARIDDggkCgsKBAQBAgASAw8CEAoMCgUEAQIABRIDDwIICgwKBQQBAgAB" + - "EgMPCQsKDAoFBAECAAMSAw8ODwoLCgQEAQIBEgMQAhAKDAoFBAECAQUSAxACCAoMCgUEAQIBARIDEAkL" + - "CgwKBQQBAgEDEgMQDg8KCwoEBAECAhIDEQITCgwKBQQBAgIFEgMRAggKDAoFBAECAgESAxEJDgoMCgUE" + - "AQICAxIDERESCgoKAgQCEgQUABYBCgoKAwQCARIDFAgrCgsKBAQCAgASAxUCGAoMCgUEAgIABRIDFQII" + - "CgwKBQQCAgABEgMVCRMKDAoFBAICAAMSAxUWFwoKCgIEAxIEGAAbAQoKCgMEAwESAxgILAoLCgQEAwIA" + - "EgMZAhIKDAoFBAMCAAUSAxkCCAoMCgUEAwIAARIDGQkNCgwKBQQDAgADEgMZEBEKCwoEBAMCARIDGgIT", - "CgwKBQQDAgEFEgMaAggKDAoFBAMCAQESAxoJDgoMCgUEAwIBAxIDGhESCgoKAgQEEgQdACABCgoKAwQE" + - "ARIDHQgzCgsKBAQEAgASAx4CEgoMCgUEBAIABRIDHgIICgwKBQQEAgABEgMeCQ0KDAoFBAQCAAMSAx4Q" + - "EQoLCgQEBAIBEgMfAhMKDAoFBAQCAQUSAx8CCAoMCgUEBAIBARIDHwkOCgwKBQQEAgEDEgMfERIKCgoC" + - "BAUSBCIAJQEKCgoDBAUBEgMiCC8KCwoEBAUCABIDIwISCgwKBQQFAgAFEgMjAggKDAoFBAUCAAESAyMJ" + - "DQoMCgUEBQIAAxIDIxARCgsKBAQFAgESAyQCEwoMCgUEBQIBBRIDJAIICgwKBQQFAgEBEgMkCQ4KDAoF" + - "BAUCAQMSAyQREgoKCgIEBhIEJwAsAQoKCgMEBgESAycIJgoLCgQEBgIAEgMoAicKDAoFBAYCAAYSAygC" + - "FwoMCgUEBgIAARIDKBgiCgwKBQQGAgADEgMoJSYKCwoEBAYCARIDKQIoCgwKBQQGAgEGEgMpAhcKDAoF" + - "BAYCAQESAykYIwoMCgUEBgIBAxIDKSYnCgsKBAQGAgISAyoCJgoMCgUEBgICBhIDKgIXCgwKBQQGAgIB" + - "EgMqGCEKDAoFBAYCAgMSAyokJQoLCgQEBgIDEgMrAicKDAoFBAYCAwYSAysCFwoMCgUEBgIDARIDKxgi" + - "CgwKBQQGAgMDEgMrJSYKCgoCBAcSBC4AMwEKCgoDBAcBEgMuCCoKCwoEBAcCABIDLwIYCgwKBQQHAgAF" + - "EgMvAggKDAoFBAcCAAESAy8JEwoMCgUEBwIAAxIDLxYXCgsKBAQHAgESAzACGQoMCgUEBwIBBRIDMAII" + - "CgwKBQQHAgEBEgMwCRQKDAoFBAcCAQMSAzAXGAoLCgQEBwICEgMxAhcKDAoFBAcCAgUSAzECCAoMCgUE" + - "BwICARIDMQkSCgwKBQQHAgIDEgMxFRYKCwoEBAcCAxIDMgIYCgwKBQQHAgMFEgMyAggKDAoFBAcCAwES" + - "AzIJEwoMCgUEBwIDAxIDMhYXCgoKAgQIEgQ1ADoBCgoKAwQIARIDNQgnCgsKBAQIAgASAzYCGAoMCgUE" + - "CAIABRIDNgIICgwKBQQIAgABEgM2CRMKDAoFBAgCAAMSAzYWFwoLCgQECAIBEgM3AhkKDAoFBAgCAQUS" + - "AzcCCAoMCgUECAIBARIDNwkUCgwKBQQIAgEDEgM3FxgKCwoEBAgCAhIDOAIXCgwKBQQIAgIFEgM4AggK" + - "DAoFBAgCAgESAzgJEgoMCgUECAICAxIDOBUWCgsKBAQIAgMSAzkCGAoMCgUECAIDBRIDOQIICgwKBQQI" + - "AgMBEgM5CRMKDAoFBAgCAwMSAzkWFwoKCgIECRIEPABBAQoKCgMECQESAzwILgoLCgQECQIAEgM9AhgK" + - "DAoFBAkCAAUSAz0CCAoMCgUECQIAARIDPQkTCgwKBQQJAgADEgM9FhcKCwoEBAkCARIDPgIZCgwKBQQJ" + - "AgEFEgM+AggKDAoFBAkCAQESAz4JFAoMCgUECQIBAxIDPhcYCgsKBAQJAgISAz8CFwoMCgUECQICBRID", - "PwIICgwKBQQJAgIBEgM/CRIKDAoFBAkCAgMSAz8VFgoLCgQECQIDEgNAAhgKDAoFBAkCAwUSA0ACCAoM" + - "CgUECQIDARIDQAkTCgwKBQQJAgMDEgNAFhcKCgoCBAoSBEMARQEKCgoDBAoBEgNDCCUKCwoEBAoCABID" + - "RAItCgwKBQQKAgAEEgNEAgoKDAoFBAoCAAYSA0QLIAoMCgUECgIAARIDRCEoCgwKBQQKAgADEgNEKywK" + - "CgoCBAsSBEcASgEKCgoDBAsBEgNHCCQKCwoEBAsCABIDSAIWCgwKBQQLAgAFEgNIAggKDAoFBAsCAAES" + - "A0gJEQoMCgUECwIAAxIDSBQVCgsKBAQLAgESA0kCHwoMCgUECwIBBhIDSQIUCgwKBQQLAgEBEgNJFRoK" + - "DAoFBAsCAQMSA0kdHgoKCgIEDBIETABPAQoKCgMEDAESA0wIIQoLCgQEDAIAEgNNAhMKDAoFBAwCAAUS" + - "A00CCAoMCgUEDAIAARIDTQkOCgwKBQQMAgADEgNNERIKCwoEBAwCARIDTgIfCgwKBQQMAgEGEgNOAhQK" + - "DAoFBAwCAQESA04VGgoMCgUEDAIBAxIDTh0eCgoKAgQNEgRRAFQBCgoKAwQNARIDUQgpCgsKBAQNAgAS" + - "A1ICGgoMCgUEDQIABRIDUgIICgwKBQQNAgABEgNSCRUKDAoFBA0CAAMSA1IYGQoLCgQEDQIBEgNTAh8K" + - "DAoFBA0CAQYSA1MCFAoMCgUEDQIBARIDUxUaCgwKBQQNAgEDEgNTHR5iBnByb3RvMw=="); + "Eh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRS" + + "aWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJS" + + "aWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURyaXZlS2luZW1hdGljcxI6Cgdtb2R1bGVzGAEgAygLMiAud3Bp" + + "LnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIHbW9kdWxlcyJvChxQcm90b2J1ZlN3ZXJ2ZU1vZHVs" + + "ZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5w" + + "cm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUS" + + "FAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90" + + "YXRpb24yZFIFYW5nbGUifAohUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb25zEiIKDGFjY2Vs" + + "ZXJhdGlvbhgBIAEoAVIMYWNjZWxlcmF0aW9uEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3Rv" + + "YnVmUm90YXRpb24yZFIFYW5nbGVCFwoVb3JnLndwaWxpYi5tYXRoLnByb3RvStYSCgYSBAAAVAEKCAoB" + + "DBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAaCggKAQgSAwYALgoJCgIIARIDBgAuCgoKAgQAEgQIAAwB" + + "CgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoMCgUEAAIABRIDCQIICgwKBQQAAgABEgMJCQsKDAoFBAAC" + + "AAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAACAQUSAwoCCAoMCgUEAAIBARIDCgkLCgwKBQQAAgEDEgMK" + + "Dg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgMLAggKDAoFBAACAgESAwsJDgoMCgUEAAICAxIDCxESCgoK" + + "AgQBEgQOABIBCgoKAwQBARIDDggkCgsKBAQBAgASAw8CEAoMCgUEAQIABRIDDwIICgwKBQQBAgABEgMP" + + "CQsKDAoFBAECAAMSAw8ODwoLCgQEAQIBEgMQAhAKDAoFBAECAQUSAxACCAoMCgUEAQIBARIDEAkLCgwK" + + "BQQBAgEDEgMQDg8KCwoEBAECAhIDEQITCgwKBQQBAgIFEgMRAggKDAoFBAECAgESAxEJDgoMCgUEAQIC" + + "AxIDERESCgoKAgQCEgQUABYBCgoKAwQCARIDFAgrCgsKBAQCAgASAxUCGAoMCgUEAgIABRIDFQIICgwK" + + "BQQCAgABEgMVCRMKDAoFBAICAAMSAxUWFwoKCgIEAxIEGAAbAQoKCgMEAwESAxgILAoLCgQEAwIAEgMZ" + + "AhIKDAoFBAMCAAUSAxkCCAoMCgUEAwIAARIDGQkNCgwKBQQDAgADEgMZEBEKCwoEBAMCARIDGgITCgwK", + "BQQDAgEFEgMaAggKDAoFBAMCAQESAxoJDgoMCgUEAwIBAxIDGhESCgoKAgQEEgQdACABCgoKAwQEARID" + + "HQgzCgsKBAQEAgASAx4CEgoMCgUEBAIABRIDHgIICgwKBQQEAgABEgMeCQ0KDAoFBAQCAAMSAx4QEQoL" + + "CgQEBAIBEgMfAhMKDAoFBAQCAQUSAx8CCAoMCgUEBAIBARIDHwkOCgwKBQQEAgEDEgMfERIKCgoCBAUS" + + "BCIAJQEKCgoDBAUBEgMiCC8KCwoEBAUCABIDIwISCgwKBQQFAgAFEgMjAggKDAoFBAUCAAESAyMJDQoM" + + "CgUEBQIAAxIDIxARCgsKBAQFAgESAyQCEwoMCgUEBQIBBRIDJAIICgwKBQQFAgEBEgMkCQ4KDAoFBAUC" + + "AQMSAyQREgoKCgIEBhIEJwAsAQoKCgMEBgESAycIJgoLCgQEBgIAEgMoAicKDAoFBAYCAAYSAygCFwoM" + + "CgUEBgIAARIDKBgiCgwKBQQGAgADEgMoJSYKCwoEBAYCARIDKQIoCgwKBQQGAgEGEgMpAhcKDAoFBAYC" + + "AQESAykYIwoMCgUEBgIBAxIDKSYnCgsKBAQGAgISAyoCJgoMCgUEBgICBhIDKgIXCgwKBQQGAgIBEgMq" + + "GCEKDAoFBAYCAgMSAyokJQoLCgQEBgIDEgMrAicKDAoFBAYCAwYSAysCFwoMCgUEBgIDARIDKxgiCgwK" + + "BQQGAgMDEgMrJSYKCgoCBAcSBC4AMwEKCgoDBAcBEgMuCCoKCwoEBAcCABIDLwIYCgwKBQQHAgAFEgMv" + + "AggKDAoFBAcCAAESAy8JEwoMCgUEBwIAAxIDLxYXCgsKBAQHAgESAzACGQoMCgUEBwIBBRIDMAIICgwK" + + "BQQHAgEBEgMwCRQKDAoFBAcCAQMSAzAXGAoLCgQEBwICEgMxAhcKDAoFBAcCAgUSAzECCAoMCgUEBwIC" + + "ARIDMQkSCgwKBQQHAgIDEgMxFRYKCwoEBAcCAxIDMgIYCgwKBQQHAgMFEgMyAggKDAoFBAcCAwESAzIJ" + + "EwoMCgUEBwIDAxIDMhYXCgoKAgQIEgQ1ADoBCgoKAwQIARIDNQgnCgsKBAQIAgASAzYCGAoMCgUECAIA" + + "BRIDNgIICgwKBQQIAgABEgM2CRMKDAoFBAgCAAMSAzYWFwoLCgQECAIBEgM3AhkKDAoFBAgCAQUSAzcC" + + "CAoMCgUECAIBARIDNwkUCgwKBQQIAgEDEgM3FxgKCwoEBAgCAhIDOAIXCgwKBQQIAgIFEgM4AggKDAoF" + + "BAgCAgESAzgJEgoMCgUECAICAxIDOBUWCgsKBAQIAgMSAzkCGAoMCgUECAIDBRIDOQIICgwKBQQIAgMB" + + "EgM5CRMKDAoFBAgCAwMSAzkWFwoKCgIECRIEPABBAQoKCgMECQESAzwILgoLCgQECQIAEgM9AhgKDAoF" + + "BAkCAAUSAz0CCAoMCgUECQIAARIDPQkTCgwKBQQJAgADEgM9FhcKCwoEBAkCARIDPgIZCgwKBQQJAgEF" + + "EgM+AggKDAoFBAkCAQESAz4JFAoMCgUECQIBAxIDPhcYCgsKBAQJAgISAz8CFwoMCgUECQICBRIDPwII", + "CgwKBQQJAgIBEgM/CRIKDAoFBAkCAgMSAz8VFgoLCgQECQIDEgNAAhgKDAoFBAkCAwUSA0ACCAoMCgUE" + + "CQIDARIDQAkTCgwKBQQJAgMDEgNAFhcKCgoCBAoSBEMARQEKCgoDBAoBEgNDCCUKCwoEBAoCABIDRAIt" + + "CgwKBQQKAgAEEgNEAgoKDAoFBAoCAAYSA0QLIAoMCgUECgIAARIDRCEoCgwKBQQKAgADEgNEKywKCgoC" + + "BAsSBEcASgEKCgoDBAsBEgNHCCQKCwoEBAsCABIDSAIWCgwKBQQLAgAFEgNIAggKDAoFBAsCAAESA0gJ" + + "EQoMCgUECwIAAxIDSBQVCgsKBAQLAgESA0kCHwoMCgUECwIBBhIDSQIUCgwKBQQLAgEBEgNJFRoKDAoF" + + "BAsCAQMSA0kdHgoKCgIEDBIETABPAQoKCgMEDAESA0wIIQoLCgQEDAIAEgNNAhMKDAoFBAwCAAUSA00C" + + "CAoMCgUEDAIAARIDTQkOCgwKBQQMAgADEgNNERIKCwoEBAwCARIDTgIfCgwKBQQMAgEGEgNOAhQKDAoF" + + "BAwCAQESA04VGgoMCgUEDAIBAxIDTh0eCgoKAgQNEgRRAFQBCgoKAwQNARIDUQgpCgsKBAQNAgASA1IC" + + "GgoMCgUEDQIABRIDUgIICgwKBQQNAgABEgNSCRUKDAoFBA0CAAMSA1IYGQoLCgQEDQIBEgNTAh8KDAoF" + + "BA0CAQYSA1MCFAoMCgUEDQIBARIDUxUaCgwKBQQNAgEDEgNTHR5iBnByb3RvMw=="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp index e2fcee2c043..1bec4ea46fb 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp @@ -188,249 +188,249 @@ static const uint8_t file_descriptor[] { 0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f, 0x62,0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f, 0x6e,0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65, -0x42,0x1a,0x0a,0x18,0x65,0x64,0x75,0x2e,0x77,0x70, -0x69,0x2e,0x66,0x69,0x72,0x73,0x74,0x2e,0x6d,0x61, -0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0xd6, -0x12,0x0a,0x06,0x12,0x04,0x00,0x00,0x54,0x01,0x0a, -0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a, -0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a, -0x09,0x0a,0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x1a, -0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x31, -0x0a,0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00, -0x31,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x08, -0x00,0x0c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01, -0x12,0x03,0x08,0x08,0x1d,0x0a,0x0b,0x0a,0x04,0x04, -0x00,0x02,0x00,0x12,0x03,0x09,0x02,0x10,0x0a,0x0c, -0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x09, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00, -0x01,0x12,0x03,0x09,0x09,0x0b,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x09,0x0e,0x0f, -0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03, -0x0a,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x01,0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0a,0x09, -0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03, -0x12,0x03,0x0a,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04, -0x00,0x02,0x02,0x12,0x03,0x0b,0x02,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x0b, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02, -0x01,0x12,0x03,0x0b,0x09,0x0e,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0b,0x11,0x12, -0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12,0x04,0x0e,0x00, -0x12,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12, -0x03,0x0e,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x01, -0x02,0x00,0x12,0x03,0x0f,0x02,0x10,0x0a,0x0c,0x0a, -0x05,0x04,0x01,0x02,0x00,0x05,0x12,0x03,0x0f,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x01, -0x12,0x03,0x0f,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04, -0x01,0x02,0x00,0x03,0x12,0x03,0x0f,0x0e,0x0f,0x0a, -0x0b,0x0a,0x04,0x04,0x01,0x02,0x01,0x12,0x03,0x10, -0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01, -0x05,0x12,0x03,0x10,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x01,0x02,0x01,0x01,0x12,0x03,0x10,0x09,0x0b, -0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x03,0x12, -0x03,0x10,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x01, -0x02,0x02,0x12,0x03,0x11,0x02,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x01,0x02,0x02,0x05,0x12,0x03,0x11,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x01, -0x12,0x03,0x11,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, -0x01,0x02,0x02,0x03,0x12,0x03,0x11,0x11,0x12,0x0a, -0x0a,0x0a,0x02,0x04,0x02,0x12,0x04,0x14,0x00,0x16, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01,0x12,0x03, -0x14,0x08,0x2b,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02, -0x00,0x12,0x03,0x15,0x02,0x18,0x0a,0x0c,0x0a,0x05, -0x04,0x02,0x02,0x00,0x05,0x12,0x03,0x15,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x01,0x12, -0x03,0x15,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02, -0x02,0x00,0x03,0x12,0x03,0x15,0x16,0x17,0x0a,0x0a, -0x0a,0x02,0x04,0x03,0x12,0x04,0x18,0x00,0x1b,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x18, -0x08,0x2c,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00, -0x12,0x03,0x19,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04, -0x03,0x02,0x00,0x05,0x12,0x03,0x19,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03, -0x19,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, -0x00,0x03,0x12,0x03,0x19,0x10,0x11,0x0a,0x0b,0x0a, -0x04,0x04,0x03,0x02,0x01,0x12,0x03,0x1a,0x02,0x13, -0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12, -0x03,0x1a,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03, -0x02,0x01,0x01,0x12,0x03,0x1a,0x09,0x0e,0x0a,0x0c, -0x0a,0x05,0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x1a, -0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04, -0x1d,0x00,0x20,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04, -0x01,0x12,0x03,0x1d,0x08,0x33,0x0a,0x0b,0x0a,0x04, -0x04,0x04,0x02,0x00,0x12,0x03,0x1e,0x02,0x12,0x0a, -0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x05,0x12,0x03, -0x1e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, -0x00,0x01,0x12,0x03,0x1e,0x09,0x0d,0x0a,0x0c,0x0a, -0x05,0x04,0x04,0x02,0x00,0x03,0x12,0x03,0x1e,0x10, -0x11,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12, -0x03,0x1f,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x04, -0x02,0x01,0x05,0x12,0x03,0x1f,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x1f, -0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01, -0x03,0x12,0x03,0x1f,0x11,0x12,0x0a,0x0a,0x0a,0x02, -0x04,0x05,0x12,0x04,0x22,0x00,0x25,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x05,0x01,0x12,0x03,0x22,0x08,0x2f, -0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03, -0x23,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, -0x00,0x05,0x12,0x03,0x23,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x05,0x02,0x00,0x01,0x12,0x03,0x23,0x09, -0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03, -0x12,0x03,0x23,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04, -0x05,0x02,0x01,0x12,0x03,0x24,0x02,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x24, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01, -0x01,0x12,0x03,0x24,0x09,0x0e,0x0a,0x0c,0x0a,0x05, -0x04,0x05,0x02,0x01,0x03,0x12,0x03,0x24,0x11,0x12, -0x0a,0x0a,0x0a,0x02,0x04,0x06,0x12,0x04,0x27,0x00, -0x2c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12, -0x03,0x27,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x06, -0x02,0x00,0x12,0x03,0x28,0x02,0x27,0x0a,0x0c,0x0a, -0x05,0x04,0x06,0x02,0x00,0x06,0x12,0x03,0x28,0x02, -0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01, -0x12,0x03,0x28,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x00,0x03,0x12,0x03,0x28,0x25,0x26,0x0a, -0x0b,0x0a,0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x29, -0x02,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01, -0x06,0x12,0x03,0x29,0x02,0x17,0x0a,0x0c,0x0a,0x05, -0x04,0x06,0x02,0x01,0x01,0x12,0x03,0x29,0x18,0x23, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12, -0x03,0x29,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06, -0x02,0x02,0x12,0x03,0x2a,0x02,0x26,0x0a,0x0c,0x0a, -0x05,0x04,0x06,0x02,0x02,0x06,0x12,0x03,0x2a,0x02, -0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01, -0x12,0x03,0x2a,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x02,0x03,0x12,0x03,0x2a,0x24,0x25,0x0a, -0x0b,0x0a,0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2b, -0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03, -0x06,0x12,0x03,0x2b,0x02,0x17,0x0a,0x0c,0x0a,0x05, -0x04,0x06,0x02,0x03,0x01,0x12,0x03,0x2b,0x18,0x22, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12, -0x03,0x2b,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x07, -0x12,0x04,0x2e,0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x07,0x01,0x12,0x03,0x2e,0x08,0x2a,0x0a,0x0b, -0x0a,0x04,0x04,0x07,0x02,0x00,0x12,0x03,0x2f,0x02, -0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x05, -0x12,0x03,0x2f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x07,0x02,0x00,0x01,0x12,0x03,0x2f,0x09,0x13,0x0a, -0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x03,0x12,0x03, -0x2f,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02, -0x01,0x12,0x03,0x30,0x02,0x19,0x0a,0x0c,0x0a,0x05, -0x04,0x07,0x02,0x01,0x05,0x12,0x03,0x30,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x01,0x12, -0x03,0x30,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07, -0x02,0x01,0x03,0x12,0x03,0x30,0x17,0x18,0x0a,0x0b, -0x0a,0x04,0x04,0x07,0x02,0x02,0x12,0x03,0x31,0x02, -0x17,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x05, -0x12,0x03,0x31,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x07,0x02,0x02,0x01,0x12,0x03,0x31,0x09,0x12,0x0a, -0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x03,0x12,0x03, -0x31,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02, -0x03,0x12,0x03,0x32,0x02,0x18,0x0a,0x0c,0x0a,0x05, -0x04,0x07,0x02,0x03,0x05,0x12,0x03,0x32,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x01,0x12, -0x03,0x32,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07, -0x02,0x03,0x03,0x12,0x03,0x32,0x16,0x17,0x0a,0x0a, -0x0a,0x02,0x04,0x08,0x12,0x04,0x35,0x00,0x3a,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x08,0x01,0x12,0x03,0x35, -0x08,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x00, -0x12,0x03,0x36,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, -0x08,0x02,0x00,0x05,0x12,0x03,0x36,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x01,0x12,0x03, -0x36,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, -0x00,0x03,0x12,0x03,0x36,0x16,0x17,0x0a,0x0b,0x0a, -0x04,0x04,0x08,0x02,0x01,0x12,0x03,0x37,0x02,0x19, -0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x05,0x12, -0x03,0x37,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08, -0x02,0x01,0x01,0x12,0x03,0x37,0x09,0x14,0x0a,0x0c, -0x0a,0x05,0x04,0x08,0x02,0x01,0x03,0x12,0x03,0x37, -0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02,0x02, -0x12,0x03,0x38,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, -0x08,0x02,0x02,0x05,0x12,0x03,0x38,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x01,0x12,0x03, -0x38,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02, -0x02,0x03,0x12,0x03,0x38,0x15,0x16,0x0a,0x0b,0x0a, -0x04,0x04,0x08,0x02,0x03,0x12,0x03,0x39,0x02,0x18, -0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x05,0x12, -0x03,0x39,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08, -0x02,0x03,0x01,0x12,0x03,0x39,0x09,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x08,0x02,0x03,0x03,0x12,0x03,0x39, -0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12,0x04, -0x3c,0x00,0x41,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09, -0x01,0x12,0x03,0x3c,0x08,0x2e,0x0a,0x0b,0x0a,0x04, -0x04,0x09,0x02,0x00,0x12,0x03,0x3d,0x02,0x18,0x0a, -0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x05,0x12,0x03, -0x3d,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, -0x00,0x01,0x12,0x03,0x3d,0x09,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x09,0x02,0x00,0x03,0x12,0x03,0x3d,0x16, -0x17,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x01,0x12, -0x03,0x3e,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09, -0x02,0x01,0x05,0x12,0x03,0x3e,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x09,0x02,0x01,0x01,0x12,0x03,0x3e, -0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01, -0x03,0x12,0x03,0x3e,0x17,0x18,0x0a,0x0b,0x0a,0x04, -0x04,0x09,0x02,0x02,0x12,0x03,0x3f,0x02,0x17,0x0a, -0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x05,0x12,0x03, -0x3f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, -0x02,0x01,0x12,0x03,0x3f,0x09,0x12,0x0a,0x0c,0x0a, -0x05,0x04,0x09,0x02,0x02,0x03,0x12,0x03,0x3f,0x15, -0x16,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x03,0x12, -0x03,0x40,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x09, -0x02,0x03,0x05,0x12,0x03,0x40,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x09,0x02,0x03,0x01,0x12,0x03,0x40, -0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03, -0x03,0x12,0x03,0x40,0x16,0x17,0x0a,0x0a,0x0a,0x02, -0x04,0x0a,0x12,0x04,0x43,0x00,0x45,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x0a,0x01,0x12,0x03,0x43,0x08,0x25, -0x0a,0x0b,0x0a,0x04,0x04,0x0a,0x02,0x00,0x12,0x03, -0x44,0x02,0x2d,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02, -0x00,0x04,0x12,0x03,0x44,0x02,0x0a,0x0a,0x0c,0x0a, -0x05,0x04,0x0a,0x02,0x00,0x06,0x12,0x03,0x44,0x0b, -0x20,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x01, -0x12,0x03,0x44,0x21,0x28,0x0a,0x0c,0x0a,0x05,0x04, -0x0a,0x02,0x00,0x03,0x12,0x03,0x44,0x2b,0x2c,0x0a, -0x0a,0x0a,0x02,0x04,0x0b,0x12,0x04,0x47,0x00,0x4a, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0b,0x01,0x12,0x03, -0x47,0x08,0x24,0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02, -0x00,0x12,0x03,0x48,0x02,0x16,0x0a,0x0c,0x0a,0x05, -0x04,0x0b,0x02,0x00,0x05,0x12,0x03,0x48,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x01,0x12, -0x03,0x48,0x09,0x11,0x0a,0x0c,0x0a,0x05,0x04,0x0b, -0x02,0x00,0x03,0x12,0x03,0x48,0x14,0x15,0x0a,0x0b, -0x0a,0x04,0x04,0x0b,0x02,0x01,0x12,0x03,0x49,0x02, -0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x06, -0x12,0x03,0x49,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04, -0x0b,0x02,0x01,0x01,0x12,0x03,0x49,0x15,0x1a,0x0a, -0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x03,0x12,0x03, -0x49,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12, -0x04,0x4c,0x00,0x4f,0x01,0x0a,0x0a,0x0a,0x03,0x04, -0x0c,0x01,0x12,0x03,0x4c,0x08,0x21,0x0a,0x0b,0x0a, -0x04,0x04,0x0c,0x02,0x00,0x12,0x03,0x4d,0x02,0x13, -0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x05,0x12, -0x03,0x4d,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0c, -0x02,0x00,0x01,0x12,0x03,0x4d,0x09,0x0e,0x0a,0x0c, -0x0a,0x05,0x04,0x0c,0x02,0x00,0x03,0x12,0x03,0x4d, -0x11,0x12,0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01, -0x12,0x03,0x4e,0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04, -0x0c,0x02,0x01,0x06,0x12,0x03,0x4e,0x02,0x14,0x0a, -0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x01,0x12,0x03, -0x4e,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02, -0x01,0x03,0x12,0x03,0x4e,0x1d,0x1e,0x0a,0x0a,0x0a, -0x02,0x04,0x0d,0x12,0x04,0x51,0x00,0x54,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x0d,0x01,0x12,0x03,0x51,0x08, -0x29,0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02,0x00,0x12, -0x03,0x52,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d, -0x02,0x00,0x05,0x12,0x03,0x52,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x0d,0x02,0x00,0x01,0x12,0x03,0x52, -0x09,0x15,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00, -0x03,0x12,0x03,0x52,0x18,0x19,0x0a,0x0b,0x0a,0x04, -0x04,0x0d,0x02,0x01,0x12,0x03,0x53,0x02,0x1f,0x0a, -0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x06,0x12,0x03, -0x53,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02, -0x01,0x01,0x12,0x03,0x53,0x15,0x1a,0x0a,0x0c,0x0a, -0x05,0x04,0x0d,0x02,0x01,0x03,0x12,0x03,0x53,0x1d, -0x1e,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, +0x42,0x17,0x0a,0x15,0x6f,0x72,0x67,0x2e,0x77,0x70, +0x69,0x6c,0x69,0x62,0x2e,0x6d,0x61,0x74,0x68,0x2e, +0x70,0x72,0x6f,0x74,0x6f,0x4a,0xd6,0x12,0x0a,0x06, +0x12,0x04,0x00,0x00,0x54,0x01,0x0a,0x08,0x0a,0x01, +0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08,0x0a,0x01, +0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x09,0x0a,0x02, +0x03,0x00,0x12,0x03,0x04,0x00,0x1a,0x0a,0x08,0x0a, +0x01,0x08,0x12,0x03,0x06,0x00,0x2e,0x0a,0x09,0x0a, +0x02,0x08,0x01,0x12,0x03,0x06,0x00,0x2e,0x0a,0x0a, +0x0a,0x02,0x04,0x00,0x12,0x04,0x08,0x00,0x0c,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12,0x03,0x08, +0x08,0x1d,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x00, +0x12,0x03,0x09,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x00,0x05,0x12,0x03,0x09,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01,0x12,0x03, +0x09,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, +0x00,0x03,0x12,0x03,0x09,0x0e,0x0f,0x0a,0x0b,0x0a, +0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x0a,0x02,0x10, +0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x05,0x12, +0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00, +0x02,0x01,0x01,0x12,0x03,0x0a,0x09,0x0b,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12,0x03,0x0a, +0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x02, +0x12,0x03,0x0b,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x02,0x05,0x12,0x03,0x0b,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01,0x12,0x03, +0x0b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, +0x02,0x03,0x12,0x03,0x0b,0x11,0x12,0x0a,0x0a,0x0a, +0x02,0x04,0x01,0x12,0x04,0x0e,0x00,0x12,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,0x03,0x0e,0x08, +0x24,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x00,0x12, +0x03,0x0f,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01, +0x02,0x00,0x05,0x12,0x03,0x0f,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x01,0x02,0x00,0x01,0x12,0x03,0x0f, +0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00, +0x03,0x12,0x03,0x0f,0x0e,0x0f,0x0a,0x0b,0x0a,0x04, +0x04,0x01,0x02,0x01,0x12,0x03,0x10,0x02,0x10,0x0a, +0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x05,0x12,0x03, +0x10,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, +0x01,0x01,0x12,0x03,0x10,0x09,0x0b,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x01,0x03,0x12,0x03,0x10,0x0e, +0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x02,0x12, +0x03,0x11,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x01, +0x02,0x02,0x05,0x12,0x03,0x11,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x01,0x02,0x02,0x01,0x12,0x03,0x11, +0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02, +0x03,0x12,0x03,0x11,0x11,0x12,0x0a,0x0a,0x0a,0x02, +0x04,0x02,0x12,0x04,0x14,0x00,0x16,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x02,0x01,0x12,0x03,0x14,0x08,0x2b, +0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x00,0x12,0x03, +0x15,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, +0x00,0x05,0x12,0x03,0x15,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x02,0x02,0x00,0x01,0x12,0x03,0x15,0x09, +0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x03, +0x12,0x03,0x15,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04, +0x03,0x12,0x04,0x18,0x00,0x1b,0x01,0x0a,0x0a,0x0a, +0x03,0x04,0x03,0x01,0x12,0x03,0x18,0x08,0x2c,0x0a, +0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12,0x03,0x19, +0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00, +0x05,0x12,0x03,0x19,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x19,0x09,0x0d, +0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x03,0x12, +0x03,0x19,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x03, +0x02,0x01,0x12,0x03,0x1a,0x02,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03,0x1a,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x01, +0x12,0x03,0x1a,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, +0x03,0x02,0x01,0x03,0x12,0x03,0x1a,0x11,0x12,0x0a, +0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1d,0x00,0x20, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01,0x12,0x03, +0x1d,0x08,0x33,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02, +0x00,0x12,0x03,0x1e,0x02,0x12,0x0a,0x0c,0x0a,0x05, +0x04,0x04,0x02,0x00,0x05,0x12,0x03,0x1e,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x01,0x12, +0x03,0x1e,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x04, +0x02,0x00,0x03,0x12,0x03,0x1e,0x10,0x11,0x0a,0x0b, +0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03,0x1f,0x02, +0x13,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x05, +0x12,0x03,0x1f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x04,0x02,0x01,0x01,0x12,0x03,0x1f,0x09,0x0e,0x0a, +0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03,0x12,0x03, +0x1f,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x05,0x12, +0x04,0x22,0x00,0x25,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x05,0x01,0x12,0x03,0x22,0x08,0x2f,0x0a,0x0b,0x0a, +0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x23,0x02,0x12, +0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x05,0x12, +0x03,0x23,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05, +0x02,0x00,0x01,0x12,0x03,0x23,0x09,0x0d,0x0a,0x0c, +0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03,0x23, +0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x01, +0x12,0x03,0x24,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04, +0x05,0x02,0x01,0x05,0x12,0x03,0x24,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01,0x12,0x03, +0x24,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, +0x01,0x03,0x12,0x03,0x24,0x11,0x12,0x0a,0x0a,0x0a, +0x02,0x04,0x06,0x12,0x04,0x27,0x00,0x2c,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03,0x27,0x08, +0x26,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x00,0x12, +0x03,0x28,0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x00,0x06,0x12,0x03,0x28,0x02,0x17,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12,0x03,0x28, +0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00, +0x03,0x12,0x03,0x28,0x25,0x26,0x0a,0x0b,0x0a,0x04, +0x04,0x06,0x02,0x01,0x12,0x03,0x29,0x02,0x28,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x06,0x12,0x03, +0x29,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x01,0x01,0x12,0x03,0x29,0x18,0x23,0x0a,0x0c,0x0a, +0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x29,0x26, +0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x02,0x12, +0x03,0x2a,0x02,0x26,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x02,0x06,0x12,0x03,0x2a,0x02,0x17,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03,0x2a, +0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02, +0x03,0x12,0x03,0x2a,0x24,0x25,0x0a,0x0b,0x0a,0x04, +0x04,0x06,0x02,0x03,0x12,0x03,0x2b,0x02,0x27,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x06,0x12,0x03, +0x2b,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x03,0x01,0x12,0x03,0x2b,0x18,0x22,0x0a,0x0c,0x0a, +0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03,0x2b,0x25, +0x26,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12,0x04,0x2e, +0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x07,0x01, +0x12,0x03,0x2e,0x08,0x2a,0x0a,0x0b,0x0a,0x04,0x04, +0x07,0x02,0x00,0x12,0x03,0x2f,0x02,0x18,0x0a,0x0c, +0x0a,0x05,0x04,0x07,0x02,0x00,0x05,0x12,0x03,0x2f, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00, +0x01,0x12,0x03,0x2f,0x09,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x07,0x02,0x00,0x03,0x12,0x03,0x2f,0x16,0x17, +0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x01,0x12,0x03, +0x30,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, +0x01,0x05,0x12,0x03,0x30,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x07,0x02,0x01,0x01,0x12,0x03,0x30,0x09, +0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x03, +0x12,0x03,0x30,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04, +0x07,0x02,0x02,0x12,0x03,0x31,0x02,0x17,0x0a,0x0c, +0x0a,0x05,0x04,0x07,0x02,0x02,0x05,0x12,0x03,0x31, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02, +0x01,0x12,0x03,0x31,0x09,0x12,0x0a,0x0c,0x0a,0x05, +0x04,0x07,0x02,0x02,0x03,0x12,0x03,0x31,0x15,0x16, +0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x03,0x12,0x03, +0x32,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, +0x03,0x05,0x12,0x03,0x32,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x07,0x02,0x03,0x01,0x12,0x03,0x32,0x09, +0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x03, +0x12,0x03,0x32,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04, +0x08,0x12,0x04,0x35,0x00,0x3a,0x01,0x0a,0x0a,0x0a, +0x03,0x04,0x08,0x01,0x12,0x03,0x35,0x08,0x27,0x0a, +0x0b,0x0a,0x04,0x04,0x08,0x02,0x00,0x12,0x03,0x36, +0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00, +0x05,0x12,0x03,0x36,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x00,0x01,0x12,0x03,0x36,0x09,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x03,0x12, +0x03,0x36,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x08, +0x02,0x01,0x12,0x03,0x37,0x02,0x19,0x0a,0x0c,0x0a, +0x05,0x04,0x08,0x02,0x01,0x05,0x12,0x03,0x37,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x01, +0x12,0x03,0x37,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x01,0x03,0x12,0x03,0x37,0x17,0x18,0x0a, +0x0b,0x0a,0x04,0x04,0x08,0x02,0x02,0x12,0x03,0x38, +0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02, +0x05,0x12,0x03,0x38,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x02,0x01,0x12,0x03,0x38,0x09,0x12, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x03,0x12, +0x03,0x38,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x08, +0x02,0x03,0x12,0x03,0x39,0x02,0x18,0x0a,0x0c,0x0a, +0x05,0x04,0x08,0x02,0x03,0x05,0x12,0x03,0x39,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x01, +0x12,0x03,0x39,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x03,0x03,0x12,0x03,0x39,0x16,0x17,0x0a, +0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,0x3c,0x00,0x41, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01,0x12,0x03, +0x3c,0x08,0x2e,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02, +0x00,0x12,0x03,0x3d,0x02,0x18,0x0a,0x0c,0x0a,0x05, +0x04,0x09,0x02,0x00,0x05,0x12,0x03,0x3d,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x01,0x12, +0x03,0x3d,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x00,0x03,0x12,0x03,0x3d,0x16,0x17,0x0a,0x0b, +0x0a,0x04,0x04,0x09,0x02,0x01,0x12,0x03,0x3e,0x02, +0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x05, +0x12,0x03,0x3e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x01,0x01,0x12,0x03,0x3e,0x09,0x14,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x03,0x12,0x03, +0x3e,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02, +0x02,0x12,0x03,0x3f,0x02,0x17,0x0a,0x0c,0x0a,0x05, +0x04,0x09,0x02,0x02,0x05,0x12,0x03,0x3f,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x01,0x12, +0x03,0x3f,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x02,0x03,0x12,0x03,0x3f,0x15,0x16,0x0a,0x0b, +0x0a,0x04,0x04,0x09,0x02,0x03,0x12,0x03,0x40,0x02, +0x18,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x05, +0x12,0x03,0x40,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x03,0x01,0x12,0x03,0x40,0x09,0x13,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x03,0x12,0x03, +0x40,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x0a,0x12, +0x04,0x43,0x00,0x45,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x0a,0x01,0x12,0x03,0x43,0x08,0x25,0x0a,0x0b,0x0a, +0x04,0x04,0x0a,0x02,0x00,0x12,0x03,0x44,0x02,0x2d, +0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x04,0x12, +0x03,0x44,0x02,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x0a, +0x02,0x00,0x06,0x12,0x03,0x44,0x0b,0x20,0x0a,0x0c, +0x0a,0x05,0x04,0x0a,0x02,0x00,0x01,0x12,0x03,0x44, +0x21,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00, +0x03,0x12,0x03,0x44,0x2b,0x2c,0x0a,0x0a,0x0a,0x02, +0x04,0x0b,0x12,0x04,0x47,0x00,0x4a,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x0b,0x01,0x12,0x03,0x47,0x08,0x24, +0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02,0x00,0x12,0x03, +0x48,0x02,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02, +0x00,0x05,0x12,0x03,0x48,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x0b,0x02,0x00,0x01,0x12,0x03,0x48,0x09, +0x11,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x03, +0x12,0x03,0x48,0x14,0x15,0x0a,0x0b,0x0a,0x04,0x04, +0x0b,0x02,0x01,0x12,0x03,0x49,0x02,0x1f,0x0a,0x0c, +0x0a,0x05,0x04,0x0b,0x02,0x01,0x06,0x12,0x03,0x49, +0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01, +0x01,0x12,0x03,0x49,0x15,0x1a,0x0a,0x0c,0x0a,0x05, +0x04,0x0b,0x02,0x01,0x03,0x12,0x03,0x49,0x1d,0x1e, +0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12,0x04,0x4c,0x00, +0x4f,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0c,0x01,0x12, +0x03,0x4c,0x08,0x21,0x0a,0x0b,0x0a,0x04,0x04,0x0c, +0x02,0x00,0x12,0x03,0x4d,0x02,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x0c,0x02,0x00,0x05,0x12,0x03,0x4d,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x01, +0x12,0x03,0x4d,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, +0x0c,0x02,0x00,0x03,0x12,0x03,0x4d,0x11,0x12,0x0a, +0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01,0x12,0x03,0x4e, +0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01, +0x06,0x12,0x03,0x4e,0x02,0x14,0x0a,0x0c,0x0a,0x05, +0x04,0x0c,0x02,0x01,0x01,0x12,0x03,0x4e,0x15,0x1a, +0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x03,0x12, +0x03,0x4e,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0d, +0x12,0x04,0x51,0x00,0x54,0x01,0x0a,0x0a,0x0a,0x03, +0x04,0x0d,0x01,0x12,0x03,0x51,0x08,0x29,0x0a,0x0b, +0x0a,0x04,0x04,0x0d,0x02,0x00,0x12,0x03,0x52,0x02, +0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x05, +0x12,0x03,0x52,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x0d,0x02,0x00,0x01,0x12,0x03,0x52,0x09,0x15,0x0a, +0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x03,0x12,0x03, +0x52,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02, +0x01,0x12,0x03,0x53,0x02,0x1f,0x0a,0x0c,0x0a,0x05, +0x04,0x0d,0x02,0x01,0x06,0x12,0x03,0x53,0x02,0x14, +0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x01,0x12, +0x03,0x53,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d, +0x02,0x01,0x03,0x12,0x03,0x53,0x1d,0x1e,0x62,0x06, +0x70,0x72,0x6f,0x74,0x6f,0x33, }; static const char file_name[] = "kinematics.proto"; static const char wpi_proto_ProtobufChassisSpeeds_name[] = "wpi.proto.ProtobufChassisSpeeds"; From 9e5634e2e86c64cb74729021ac2b4c087a94784a Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 25 Nov 2025 12:50:27 -0500 Subject: [PATCH 77/82] rename SwerveModuleAccelerations to SwerveModuleAccelerations Signed-off-by: Zach Harel --- .../org/wpilib/math/proto/Kinematics.java | 159 +++--- .../cpp/wpimath/protobuf/kinematics.npb.cpp | 514 +++++++++--------- .../cpp/wpimath/protobuf/kinematics.npb.h | 22 +- .../kinematics/SwerveDriveKinematics.java | 18 +- ...ons.java => SwerveModuleAcceleration.java} | 30 +- ...ava => SwerveModuleAccelerationProto.java} | 16 +- ...va => SwerveModuleAccelerationStruct.java} | 14 +- ....cpp => SwerveModuleAccelerationProto.cpp} | 13 +- ...cpp => SwerveModuleAccelerationStruct.cpp} | 15 +- .../estimator/SwerveDrivePoseEstimator.hpp | 2 +- .../estimator/SwerveDrivePoseEstimator3d.hpp | 2 +- .../math/kinematics/SwerveDriveKinematics.hpp | 18 +- .../math/kinematics/SwerveDriveOdometry.hpp | 2 +- .../math/kinematics/SwerveDriveOdometry3d.hpp | 2 +- ...tions.hpp => SwerveModuleAcceleration.hpp} | 22 +- ....hpp => SwerveModuleAccelerationProto.hpp} | 12 +- ...hpp => SwerveModuleAccelerationStruct.hpp} | 12 +- wpimath/src/main/proto/kinematics.proto | 2 +- .../kinematics/SwerveDriveKinematicsTest.java | 16 +- ...java => SwerveModuleAccelerationTest.java} | 24 +- .../kinematics/SwerveDriveKinematicsTest.cpp | 8 +- .../SwerveModuleAccelerationsTest.cpp | 13 +- 22 files changed, 466 insertions(+), 470 deletions(-) rename wpimath/src/main/java/org/wpilib/math/kinematics/{SwerveModuleAccelerations.java => SwerveModuleAcceleration.java} (78%) rename wpimath/src/main/java/org/wpilib/math/kinematics/proto/{SwerveModuleAccelerationsProto.java => SwerveModuleAccelerationProto.java} (68%) rename wpimath/src/main/java/org/wpilib/math/kinematics/struct/{SwerveModuleAccelerationsStruct.java => SwerveModuleAccelerationStruct.java} (68%) rename wpimath/src/main/native/cpp/kinematics/proto/{SwerveModuleAccelerationsProto.cpp => SwerveModuleAccelerationProto.cpp} (73%) rename wpimath/src/main/native/cpp/kinematics/struct/{SwerveModuleAccelerationsStruct.cpp => SwerveModuleAccelerationStruct.cpp} (66%) rename wpimath/src/main/native/include/wpi/math/kinematics/{SwerveModuleAccelerations.hpp => SwerveModuleAcceleration.hpp} (81%) rename wpimath/src/main/native/include/wpi/math/kinematics/proto/{SwerveModuleAccelerationsProto.hpp => SwerveModuleAccelerationProto.hpp} (70%) rename wpimath/src/main/native/include/wpi/math/kinematics/struct/{SwerveModuleAccelerationsStruct.hpp => SwerveModuleAccelerationStruct.hpp} (78%) rename wpimath/src/test/java/org/wpilib/math/kinematics/{SwerveModuleAccelerationsTest.java => SwerveModuleAccelerationTest.java} (80%) diff --git a/wpimath/src/generated/main/java/org/wpilib/math/proto/Kinematics.java b/wpimath/src/generated/main/java/org/wpilib/math/proto/Kinematics.java index bd330165a9b..0969647a8d1 100644 --- a/wpimath/src/generated/main/java/org/wpilib/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/org/wpilib/math/proto/Kinematics.java @@ -19,7 +19,7 @@ import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4186, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4185, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + "BW9tZWdhIlQKHFByb3RvYnVmQ2hhc3Npc0FjY2VsZXJhdGlvbnMSDgoCYXgYASABKAFSAmF4Eg4KAmF5" + @@ -47,49 +47,49 @@ public final class Kinematics { "ZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5w" + "cm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUS" + "FAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90" + - "YXRpb24yZFIFYW5nbGUifAohUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb25zEiIKDGFjY2Vs" + - "ZXJhdGlvbhgBIAEoAVIMYWNjZWxlcmF0aW9uEjMKBWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3Rv" + - "YnVmUm90YXRpb24yZFIFYW5nbGVCFwoVb3JnLndwaWxpYi5tYXRoLnByb3RvStYSCgYSBAAAVAEKCAoB" + - "DBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAaCggKAQgSAwYALgoJCgIIARIDBgAuCgoKAgQAEgQIAAwB" + - "CgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoMCgUEAAIABRIDCQIICgwKBQQAAgABEgMJCQsKDAoFBAAC" + - "AAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAACAQUSAwoCCAoMCgUEAAIBARIDCgkLCgwKBQQAAgEDEgMK" + - "Dg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgMLAggKDAoFBAACAgESAwsJDgoMCgUEAAICAxIDCxESCgoK" + - "AgQBEgQOABIBCgoKAwQBARIDDggkCgsKBAQBAgASAw8CEAoMCgUEAQIABRIDDwIICgwKBQQBAgABEgMP" + - "CQsKDAoFBAECAAMSAw8ODwoLCgQEAQIBEgMQAhAKDAoFBAECAQUSAxACCAoMCgUEAQIBARIDEAkLCgwK" + - "BQQBAgEDEgMQDg8KCwoEBAECAhIDEQITCgwKBQQBAgIFEgMRAggKDAoFBAECAgESAxEJDgoMCgUEAQIC" + - "AxIDERESCgoKAgQCEgQUABYBCgoKAwQCARIDFAgrCgsKBAQCAgASAxUCGAoMCgUEAgIABRIDFQIICgwK" + - "BQQCAgABEgMVCRMKDAoFBAICAAMSAxUWFwoKCgIEAxIEGAAbAQoKCgMEAwESAxgILAoLCgQEAwIAEgMZ" + - "AhIKDAoFBAMCAAUSAxkCCAoMCgUEAwIAARIDGQkNCgwKBQQDAgADEgMZEBEKCwoEBAMCARIDGgITCgwK", - "BQQDAgEFEgMaAggKDAoFBAMCAQESAxoJDgoMCgUEAwIBAxIDGhESCgoKAgQEEgQdACABCgoKAwQEARID" + - "HQgzCgsKBAQEAgASAx4CEgoMCgUEBAIABRIDHgIICgwKBQQEAgABEgMeCQ0KDAoFBAQCAAMSAx4QEQoL" + - "CgQEBAIBEgMfAhMKDAoFBAQCAQUSAx8CCAoMCgUEBAIBARIDHwkOCgwKBQQEAgEDEgMfERIKCgoCBAUS" + - "BCIAJQEKCgoDBAUBEgMiCC8KCwoEBAUCABIDIwISCgwKBQQFAgAFEgMjAggKDAoFBAUCAAESAyMJDQoM" + - "CgUEBQIAAxIDIxARCgsKBAQFAgESAyQCEwoMCgUEBQIBBRIDJAIICgwKBQQFAgEBEgMkCQ4KDAoFBAUC" + - "AQMSAyQREgoKCgIEBhIEJwAsAQoKCgMEBgESAycIJgoLCgQEBgIAEgMoAicKDAoFBAYCAAYSAygCFwoM" + - "CgUEBgIAARIDKBgiCgwKBQQGAgADEgMoJSYKCwoEBAYCARIDKQIoCgwKBQQGAgEGEgMpAhcKDAoFBAYC" + - "AQESAykYIwoMCgUEBgIBAxIDKSYnCgsKBAQGAgISAyoCJgoMCgUEBgICBhIDKgIXCgwKBQQGAgIBEgMq" + - "GCEKDAoFBAYCAgMSAyokJQoLCgQEBgIDEgMrAicKDAoFBAYCAwYSAysCFwoMCgUEBgIDARIDKxgiCgwK" + - "BQQGAgMDEgMrJSYKCgoCBAcSBC4AMwEKCgoDBAcBEgMuCCoKCwoEBAcCABIDLwIYCgwKBQQHAgAFEgMv" + - "AggKDAoFBAcCAAESAy8JEwoMCgUEBwIAAxIDLxYXCgsKBAQHAgESAzACGQoMCgUEBwIBBRIDMAIICgwK" + - "BQQHAgEBEgMwCRQKDAoFBAcCAQMSAzAXGAoLCgQEBwICEgMxAhcKDAoFBAcCAgUSAzECCAoMCgUEBwIC" + - "ARIDMQkSCgwKBQQHAgIDEgMxFRYKCwoEBAcCAxIDMgIYCgwKBQQHAgMFEgMyAggKDAoFBAcCAwESAzIJ" + - "EwoMCgUEBwIDAxIDMhYXCgoKAgQIEgQ1ADoBCgoKAwQIARIDNQgnCgsKBAQIAgASAzYCGAoMCgUECAIA" + - "BRIDNgIICgwKBQQIAgABEgM2CRMKDAoFBAgCAAMSAzYWFwoLCgQECAIBEgM3AhkKDAoFBAgCAQUSAzcC" + - "CAoMCgUECAIBARIDNwkUCgwKBQQIAgEDEgM3FxgKCwoEBAgCAhIDOAIXCgwKBQQIAgIFEgM4AggKDAoF" + - "BAgCAgESAzgJEgoMCgUECAICAxIDOBUWCgsKBAQIAgMSAzkCGAoMCgUECAIDBRIDOQIICgwKBQQIAgMB" + - "EgM5CRMKDAoFBAgCAwMSAzkWFwoKCgIECRIEPABBAQoKCgMECQESAzwILgoLCgQECQIAEgM9AhgKDAoF" + - "BAkCAAUSAz0CCAoMCgUECQIAARIDPQkTCgwKBQQJAgADEgM9FhcKCwoEBAkCARIDPgIZCgwKBQQJAgEF" + - "EgM+AggKDAoFBAkCAQESAz4JFAoMCgUECQIBAxIDPhcYCgsKBAQJAgISAz8CFwoMCgUECQICBRIDPwII", - "CgwKBQQJAgIBEgM/CRIKDAoFBAkCAgMSAz8VFgoLCgQECQIDEgNAAhgKDAoFBAkCAwUSA0ACCAoMCgUE" + - "CQIDARIDQAkTCgwKBQQJAgMDEgNAFhcKCgoCBAoSBEMARQEKCgoDBAoBEgNDCCUKCwoEBAoCABIDRAIt" + - "CgwKBQQKAgAEEgNEAgoKDAoFBAoCAAYSA0QLIAoMCgUECgIAARIDRCEoCgwKBQQKAgADEgNEKywKCgoC" + - "BAsSBEcASgEKCgoDBAsBEgNHCCQKCwoEBAsCABIDSAIWCgwKBQQLAgAFEgNIAggKDAoFBAsCAAESA0gJ" + - "EQoMCgUECwIAAxIDSBQVCgsKBAQLAgESA0kCHwoMCgUECwIBBhIDSQIUCgwKBQQLAgEBEgNJFRoKDAoF" + - "BAsCAQMSA0kdHgoKCgIEDBIETABPAQoKCgMEDAESA0wIIQoLCgQEDAIAEgNNAhMKDAoFBAwCAAUSA00C" + - "CAoMCgUEDAIAARIDTQkOCgwKBQQMAgADEgNNERIKCwoEBAwCARIDTgIfCgwKBQQMAgEGEgNOAhQKDAoF" + - "BAwCAQESA04VGgoMCgUEDAIBAxIDTh0eCgoKAgQNEgRRAFQBCgoKAwQNARIDUQgpCgsKBAQNAgASA1IC" + - "GgoMCgUEDQIABRIDUgIICgwKBQQNAgABEgNSCRUKDAoFBA0CAAMSA1IYGQoLCgQEDQIBEgNTAh8KDAoF" + - "BA0CAQYSA1MCFAoMCgUEDQIBARIDUxUaCgwKBQQNAgEDEgNTHR5iBnByb3RvMw=="); + "YXRpb24yZFIFYW5nbGUiewogUHJvdG9idWZTd2VydmVNb2R1bGVBY2NlbGVyYXRpb24SIgoMYWNjZWxl" + + "cmF0aW9uGAEgASgBUgxhY2NlbGVyYXRpb24SMwoFYW5nbGUYAiABKAsyHS53cGkucHJvdG8uUHJvdG9i" + + "dWZSb3RhdGlvbjJkUgVhbmdsZUIXChVvcmcud3BpbGliLm1hdGgucHJvdG9K1hIKBhIEAABUAQoICgEM" + + "EgMAABIKCAoBAhIDAgASCgkKAgMAEgMEABoKCAoBCBIDBgAuCgkKAggBEgMGAC4KCgoCBAASBAgADAEK" + + "CgoDBAABEgMICB0KCwoEBAACABIDCQIQCgwKBQQAAgAFEgMJAggKDAoFBAACAAESAwkJCwoMCgUEAAIA" + + "AxIDCQ4PCgsKBAQAAgESAwoCEAoMCgUEAAIBBRIDCgIICgwKBQQAAgEBEgMKCQsKDAoFBAACAQMSAwoO" + + "DwoLCgQEAAICEgMLAhMKDAoFBAACAgUSAwsCCAoMCgUEAAICARIDCwkOCgwKBQQAAgIDEgMLERIKCgoC" + + "BAESBA4AEgEKCgoDBAEBEgMOCCQKCwoEBAECABIDDwIQCgwKBQQBAgAFEgMPAggKDAoFBAECAAESAw8J" + + "CwoMCgUEAQIAAxIDDw4PCgsKBAQBAgESAxACEAoMCgUEAQIBBRIDEAIICgwKBQQBAgEBEgMQCQsKDAoF" + + "BAECAQMSAxAODwoLCgQEAQICEgMRAhMKDAoFBAECAgUSAxECCAoMCgUEAQICARIDEQkOCgwKBQQBAgID" + + "EgMRERIKCgoCBAISBBQAFgEKCgoDBAIBEgMUCCsKCwoEBAICABIDFQIYCgwKBQQCAgAFEgMVAggKDAoF" + + "BAICAAESAxUJEwoMCgUEAgIAAxIDFRYXCgoKAgQDEgQYABsBCgoKAwQDARIDGAgsCgsKBAQDAgASAxkC" + + "EgoMCgUEAwIABRIDGQIICgwKBQQDAgABEgMZCQ0KDAoFBAMCAAMSAxkQEQoLCgQEAwIBEgMaAhMKDAoF", + "BAMCAQUSAxoCCAoMCgUEAwIBARIDGgkOCgwKBQQDAgEDEgMaERIKCgoCBAQSBB0AIAEKCgoDBAQBEgMd" + + "CDMKCwoEBAQCABIDHgISCgwKBQQEAgAFEgMeAggKDAoFBAQCAAESAx4JDQoMCgUEBAIAAxIDHhARCgsK" + + "BAQEAgESAx8CEwoMCgUEBAIBBRIDHwIICgwKBQQEAgEBEgMfCQ4KDAoFBAQCAQMSAx8REgoKCgIEBRIE" + + "IgAlAQoKCgMEBQESAyIILwoLCgQEBQIAEgMjAhIKDAoFBAUCAAUSAyMCCAoMCgUEBQIAARIDIwkNCgwK" + + "BQQFAgADEgMjEBEKCwoEBAUCARIDJAITCgwKBQQFAgEFEgMkAggKDAoFBAUCAQESAyQJDgoMCgUEBQIB" + + "AxIDJBESCgoKAgQGEgQnACwBCgoKAwQGARIDJwgmCgsKBAQGAgASAygCJwoMCgUEBgIABhIDKAIXCgwK" + + "BQQGAgABEgMoGCIKDAoFBAYCAAMSAyglJgoLCgQEBgIBEgMpAigKDAoFBAYCAQYSAykCFwoMCgUEBgIB" + + "ARIDKRgjCgwKBQQGAgEDEgMpJicKCwoEBAYCAhIDKgImCgwKBQQGAgIGEgMqAhcKDAoFBAYCAgESAyoY" + + "IQoMCgUEBgICAxIDKiQlCgsKBAQGAgMSAysCJwoMCgUEBgIDBhIDKwIXCgwKBQQGAgMBEgMrGCIKDAoF" + + "BAYCAwMSAyslJgoKCgIEBxIELgAzAQoKCgMEBwESAy4IKgoLCgQEBwIAEgMvAhgKDAoFBAcCAAUSAy8C" + + "CAoMCgUEBwIAARIDLwkTCgwKBQQHAgADEgMvFhcKCwoEBAcCARIDMAIZCgwKBQQHAgEFEgMwAggKDAoF" + + "BAcCAQESAzAJFAoMCgUEBwIBAxIDMBcYCgsKBAQHAgISAzECFwoMCgUEBwICBRIDMQIICgwKBQQHAgIB" + + "EgMxCRIKDAoFBAcCAgMSAzEVFgoLCgQEBwIDEgMyAhgKDAoFBAcCAwUSAzICCAoMCgUEBwIDARIDMgkT" + + "CgwKBQQHAgMDEgMyFhcKCgoCBAgSBDUAOgEKCgoDBAgBEgM1CCcKCwoEBAgCABIDNgIYCgwKBQQIAgAF" + + "EgM2AggKDAoFBAgCAAESAzYJEwoMCgUECAIAAxIDNhYXCgsKBAQIAgESAzcCGQoMCgUECAIBBRIDNwII" + + "CgwKBQQIAgEBEgM3CRQKDAoFBAgCAQMSAzcXGAoLCgQECAICEgM4AhcKDAoFBAgCAgUSAzgCCAoMCgUE" + + "CAICARIDOAkSCgwKBQQIAgIDEgM4FRYKCwoEBAgCAxIDOQIYCgwKBQQIAgMFEgM5AggKDAoFBAgCAwES" + + "AzkJEwoMCgUECAIDAxIDORYXCgoKAgQJEgQ8AEEBCgoKAwQJARIDPAguCgsKBAQJAgASAz0CGAoMCgUE" + + "CQIABRIDPQIICgwKBQQJAgABEgM9CRMKDAoFBAkCAAMSAz0WFwoLCgQECQIBEgM+AhkKDAoFBAkCAQUS" + + "Az4CCAoMCgUECQIBARIDPgkUCgwKBQQJAgEDEgM+FxgKCwoEBAkCAhIDPwIXCgwKBQQJAgIFEgM/AggK", + "DAoFBAkCAgESAz8JEgoMCgUECQICAxIDPxUWCgsKBAQJAgMSA0ACGAoMCgUECQIDBRIDQAIICgwKBQQJ" + + "AgMBEgNACRMKDAoFBAkCAwMSA0AWFwoKCgIEChIEQwBFAQoKCgMECgESA0MIJQoLCgQECgIAEgNEAi0K" + + "DAoFBAoCAAQSA0QCCgoMCgUECgIABhIDRAsgCgwKBQQKAgABEgNEISgKDAoFBAoCAAMSA0QrLAoKCgIE" + + "CxIERwBKAQoKCgMECwESA0cIJAoLCgQECwIAEgNIAhYKDAoFBAsCAAUSA0gCCAoMCgUECwIAARIDSAkR" + + "CgwKBQQLAgADEgNIFBUKCwoEBAsCARIDSQIfCgwKBQQLAgEGEgNJAhQKDAoFBAsCAQESA0kVGgoMCgUE" + + "CwIBAxIDSR0eCgoKAgQMEgRMAE8BCgoKAwQMARIDTAghCgsKBAQMAgASA00CEwoMCgUEDAIABRIDTQII" + + "CgwKBQQMAgABEgNNCQ4KDAoFBAwCAAMSA00REgoLCgQEDAIBEgNOAh8KDAoFBAwCAQYSA04CFAoMCgUE" + + "DAIBARIDThUaCgwKBQQMAgEDEgNOHR4KCgoCBA0SBFEAVAEKCgoDBA0BEgNRCCgKCwoEBA0CABIDUgIa" + + "CgwKBQQNAgAFEgNSAggKDAoFBA0CAAESA1IJFQoMCgUEDQIAAxIDUhgZCgsKBAQNAgESA1MCHwoMCgUE" + + "DQIBBhIDUwIUCgwKBQQNAgEBEgNTFRoKDAoFBA0CAQMSA1MdHmIGcHJvdG8z"); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); @@ -119,7 +119,7 @@ public final class Kinematics { static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1532, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleAccelerations_descriptor = descriptor.internalContainedType(1636, 124, "ProtobufSwerveModuleAccelerations", "wpi.proto.ProtobufSwerveModuleAccelerations"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleAcceleration_descriptor = descriptor.internalContainedType(1636, 123, "ProtobufSwerveModuleAcceleration", "wpi.proto.ProtobufSwerveModuleAcceleration"); /** * @return this proto file's descriptor. @@ -5310,9 +5310,9 @@ static class FieldNames { } /** - * Protobuf type {@code ProtobufSwerveModuleAccelerations} + * Protobuf type {@code ProtobufSwerveModuleAcceleration} */ - public static final class ProtobufSwerveModuleAccelerations extends ProtoMessage implements Cloneable { + public static final class ProtobufSwerveModuleAcceleration extends ProtoMessage implements Cloneable { private static final long serialVersionUID = 0L; /** @@ -5325,14 +5325,14 @@ public static final class ProtobufSwerveModuleAccelerations extends ProtoMessage */ private final Geometry2D.ProtobufRotation2d angle = Geometry2D.ProtobufRotation2d.newInstance(); - private ProtobufSwerveModuleAccelerations() { + private ProtobufSwerveModuleAcceleration() { } /** - * @return a new empty instance of {@code ProtobufSwerveModuleAccelerations} + * @return a new empty instance of {@code ProtobufSwerveModuleAcceleration} */ - public static ProtobufSwerveModuleAccelerations newInstance() { - return new ProtobufSwerveModuleAccelerations(); + public static ProtobufSwerveModuleAcceleration newInstance() { + return new ProtobufSwerveModuleAcceleration(); } /** @@ -5347,7 +5347,7 @@ public boolean hasAcceleration() { * optional double acceleration = 1; * @return this */ - public ProtobufSwerveModuleAccelerations clearAcceleration() { + public ProtobufSwerveModuleAcceleration clearAcceleration() { bitField0_ &= ~0x00000001; acceleration = 0D; return this; @@ -5366,7 +5366,7 @@ public double getAcceleration() { * @param value the acceleration to set * @return this */ - public ProtobufSwerveModuleAccelerations setAcceleration(final double value) { + public ProtobufSwerveModuleAcceleration setAcceleration(final double value) { bitField0_ |= 0x00000001; acceleration = value; return this; @@ -5384,7 +5384,7 @@ public boolean hasAngle() { * optional .wpi.proto.ProtobufRotation2d angle = 2; * @return this */ - public ProtobufSwerveModuleAccelerations clearAngle() { + public ProtobufSwerveModuleAcceleration clearAngle() { bitField0_ &= ~0x00000002; angle.clear(); return this; @@ -5423,15 +5423,14 @@ public Geometry2D.ProtobufRotation2d getMutableAngle() { * @param value the angle to set * @return this */ - public ProtobufSwerveModuleAccelerations setAngle(final Geometry2D.ProtobufRotation2d value) { + public ProtobufSwerveModuleAcceleration setAngle(final Geometry2D.ProtobufRotation2d value) { bitField0_ |= 0x00000002; angle.copyFrom(value); return this; } @Override - public ProtobufSwerveModuleAccelerations copyFrom( - final ProtobufSwerveModuleAccelerations other) { + public ProtobufSwerveModuleAcceleration copyFrom(final ProtobufSwerveModuleAcceleration other) { cachedSize = other.cachedSize; if ((bitField0_ | other.bitField0_) != 0) { bitField0_ = other.bitField0_; @@ -5442,8 +5441,8 @@ public ProtobufSwerveModuleAccelerations copyFrom( } @Override - public ProtobufSwerveModuleAccelerations mergeFrom( - final ProtobufSwerveModuleAccelerations other) { + public ProtobufSwerveModuleAcceleration mergeFrom( + final ProtobufSwerveModuleAcceleration other) { if (other.isEmpty()) { return this; } @@ -5458,7 +5457,7 @@ public ProtobufSwerveModuleAccelerations mergeFrom( } @Override - public ProtobufSwerveModuleAccelerations clear() { + public ProtobufSwerveModuleAcceleration clear() { if (isEmpty()) { return this; } @@ -5470,7 +5469,7 @@ public ProtobufSwerveModuleAccelerations clear() { } @Override - public ProtobufSwerveModuleAccelerations clearQuick() { + public ProtobufSwerveModuleAcceleration clearQuick() { if (isEmpty()) { return this; } @@ -5485,10 +5484,10 @@ public boolean equals(Object o) { if (o == this) { return true; } - if (!(o instanceof ProtobufSwerveModuleAccelerations)) { + if (!(o instanceof ProtobufSwerveModuleAcceleration)) { return false; } - ProtobufSwerveModuleAccelerations other = (ProtobufSwerveModuleAccelerations) o; + ProtobufSwerveModuleAcceleration other = (ProtobufSwerveModuleAcceleration) o; return bitField0_ == other.bitField0_ && (!hasAcceleration() || ProtoUtil.isEqual(acceleration, other.acceleration)) && (!hasAngle() || angle.equals(other.angle)); @@ -5520,7 +5519,7 @@ protected int computeSerializedSize() { @Override @SuppressWarnings("fallthrough") - public ProtobufSwerveModuleAccelerations mergeFrom(final ProtoSource input) throws IOException { + public ProtobufSwerveModuleAcceleration mergeFrom(final ProtoSource input) throws IOException { // Enabled Fall-Through Optimization (QuickBuffers) int tag = input.readTag(); while (true) { @@ -5570,7 +5569,7 @@ public void writeTo(final JsonSink output) throws IOException { } @Override - public ProtobufSwerveModuleAccelerations mergeFrom(final JsonSource input) throws IOException { + public ProtobufSwerveModuleAcceleration mergeFrom(final JsonSource input) throws IOException { if (!input.beginObject()) { return this; } @@ -5609,8 +5608,8 @@ public ProtobufSwerveModuleAccelerations mergeFrom(final JsonSource input) throw } @Override - public ProtobufSwerveModuleAccelerations clone() { - return new ProtobufSwerveModuleAccelerations().copyFrom(this); + public ProtobufSwerveModuleAcceleration clone() { + return new ProtobufSwerveModuleAcceleration().copyFrom(this); } @Override @@ -5618,41 +5617,41 @@ public boolean isEmpty() { return ((bitField0_) == 0); } - public static ProtobufSwerveModuleAccelerations parseFrom(final byte[] data) throws + public static ProtobufSwerveModuleAcceleration parseFrom(final byte[] data) throws InvalidProtocolBufferException { - return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAccelerations(), data).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAcceleration(), data).checkInitialized(); } - public static ProtobufSwerveModuleAccelerations parseFrom(final ProtoSource input) throws + public static ProtobufSwerveModuleAcceleration parseFrom(final ProtoSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAccelerations(), input).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAcceleration(), input).checkInitialized(); } - public static ProtobufSwerveModuleAccelerations parseFrom(final JsonSource input) throws + public static ProtobufSwerveModuleAcceleration parseFrom(final JsonSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAccelerations(), input).checkInitialized(); + return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAcceleration(), input).checkInitialized(); } /** - * @return factory for creating ProtobufSwerveModuleAccelerations messages + * @return factory for creating ProtobufSwerveModuleAcceleration messages */ - public static MessageFactory getFactory() { - return ProtobufSwerveModuleAccelerationsFactory.INSTANCE; + public static MessageFactory getFactory() { + return ProtobufSwerveModuleAccelerationFactory.INSTANCE; } /** * @return this type's descriptor. */ public static Descriptors.Descriptor getDescriptor() { - return Kinematics.wpi_proto_ProtobufSwerveModuleAccelerations_descriptor; + return Kinematics.wpi_proto_ProtobufSwerveModuleAcceleration_descriptor; } - private enum ProtobufSwerveModuleAccelerationsFactory implements MessageFactory { + private enum ProtobufSwerveModuleAccelerationFactory implements MessageFactory { INSTANCE; @Override - public ProtobufSwerveModuleAccelerations create() { - return ProtobufSwerveModuleAccelerations.newInstance(); + public ProtobufSwerveModuleAcceleration create() { + return ProtobufSwerveModuleAcceleration.newInstance(); } } diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp index 1bec4ea46fb..e60b09a19e3 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp @@ -175,262 +175,262 @@ static const uint8_t file_descriptor[] { 0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50, 0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74, 0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61, -0x6e,0x67,0x6c,0x65,0x22,0x7c,0x0a,0x21,0x50,0x72, +0x6e,0x67,0x6c,0x65,0x22,0x7b,0x0a,0x20,0x50,0x72, 0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72, 0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65,0x41,0x63, 0x63,0x65,0x6c,0x65,0x72,0x61,0x74,0x69,0x6f,0x6e, -0x73,0x12,0x22,0x0a,0x0c,0x61,0x63,0x63,0x65,0x6c, -0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x18,0x01,0x20, -0x01,0x28,0x01,0x52,0x0c,0x61,0x63,0x63,0x65,0x6c, -0x65,0x72,0x61,0x74,0x69,0x6f,0x6e,0x12,0x33,0x0a, -0x05,0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01, -0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70, -0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f, -0x62,0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f, -0x6e,0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65, -0x42,0x17,0x0a,0x15,0x6f,0x72,0x67,0x2e,0x77,0x70, -0x69,0x6c,0x69,0x62,0x2e,0x6d,0x61,0x74,0x68,0x2e, -0x70,0x72,0x6f,0x74,0x6f,0x4a,0xd6,0x12,0x0a,0x06, -0x12,0x04,0x00,0x00,0x54,0x01,0x0a,0x08,0x0a,0x01, -0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08,0x0a,0x01, -0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x09,0x0a,0x02, -0x03,0x00,0x12,0x03,0x04,0x00,0x1a,0x0a,0x08,0x0a, -0x01,0x08,0x12,0x03,0x06,0x00,0x2e,0x0a,0x09,0x0a, -0x02,0x08,0x01,0x12,0x03,0x06,0x00,0x2e,0x0a,0x0a, -0x0a,0x02,0x04,0x00,0x12,0x04,0x08,0x00,0x0c,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12,0x03,0x08, -0x08,0x1d,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x00, -0x12,0x03,0x09,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x00,0x05,0x12,0x03,0x09,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01,0x12,0x03, -0x09,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x00,0x03,0x12,0x03,0x09,0x0e,0x0f,0x0a,0x0b,0x0a, -0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x0a,0x02,0x10, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x05,0x12, -0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x01,0x01,0x12,0x03,0x0a,0x09,0x0b,0x0a,0x0c, -0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12,0x03,0x0a, -0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x02, -0x12,0x03,0x0b,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x02,0x05,0x12,0x03,0x0b,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01,0x12,0x03, -0x0b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x02,0x03,0x12,0x03,0x0b,0x11,0x12,0x0a,0x0a,0x0a, -0x02,0x04,0x01,0x12,0x04,0x0e,0x00,0x12,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,0x03,0x0e,0x08, -0x24,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x00,0x12, -0x03,0x0f,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01, -0x02,0x00,0x05,0x12,0x03,0x0f,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x01,0x02,0x00,0x01,0x12,0x03,0x0f, -0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00, -0x03,0x12,0x03,0x0f,0x0e,0x0f,0x0a,0x0b,0x0a,0x04, -0x04,0x01,0x02,0x01,0x12,0x03,0x10,0x02,0x10,0x0a, -0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x05,0x12,0x03, -0x10,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, -0x01,0x01,0x12,0x03,0x10,0x09,0x0b,0x0a,0x0c,0x0a, -0x05,0x04,0x01,0x02,0x01,0x03,0x12,0x03,0x10,0x0e, -0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x02,0x12, -0x03,0x11,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x01, -0x02,0x02,0x05,0x12,0x03,0x11,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x01,0x02,0x02,0x01,0x12,0x03,0x11, -0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02, -0x03,0x12,0x03,0x11,0x11,0x12,0x0a,0x0a,0x0a,0x02, -0x04,0x02,0x12,0x04,0x14,0x00,0x16,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x02,0x01,0x12,0x03,0x14,0x08,0x2b, -0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x00,0x12,0x03, -0x15,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, -0x00,0x05,0x12,0x03,0x15,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x02,0x02,0x00,0x01,0x12,0x03,0x15,0x09, -0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x03, -0x12,0x03,0x15,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04, -0x03,0x12,0x04,0x18,0x00,0x1b,0x01,0x0a,0x0a,0x0a, -0x03,0x04,0x03,0x01,0x12,0x03,0x18,0x08,0x2c,0x0a, -0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12,0x03,0x19, -0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00, -0x05,0x12,0x03,0x19,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x19,0x09,0x0d, -0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x03,0x12, -0x03,0x19,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x03, -0x02,0x01,0x12,0x03,0x1a,0x02,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03,0x1a,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x01, -0x12,0x03,0x1a,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, -0x03,0x02,0x01,0x03,0x12,0x03,0x1a,0x11,0x12,0x0a, -0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1d,0x00,0x20, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01,0x12,0x03, -0x1d,0x08,0x33,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02, -0x00,0x12,0x03,0x1e,0x02,0x12,0x0a,0x0c,0x0a,0x05, -0x04,0x04,0x02,0x00,0x05,0x12,0x03,0x1e,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x01,0x12, -0x03,0x1e,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x04, -0x02,0x00,0x03,0x12,0x03,0x1e,0x10,0x11,0x0a,0x0b, -0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03,0x1f,0x02, -0x13,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x05, -0x12,0x03,0x1f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x04,0x02,0x01,0x01,0x12,0x03,0x1f,0x09,0x0e,0x0a, -0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03,0x12,0x03, -0x1f,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x05,0x12, -0x04,0x22,0x00,0x25,0x01,0x0a,0x0a,0x0a,0x03,0x04, -0x05,0x01,0x12,0x03,0x22,0x08,0x2f,0x0a,0x0b,0x0a, -0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x23,0x02,0x12, -0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x05,0x12, -0x03,0x23,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05, -0x02,0x00,0x01,0x12,0x03,0x23,0x09,0x0d,0x0a,0x0c, -0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03,0x23, -0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x01, -0x12,0x03,0x24,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04, -0x05,0x02,0x01,0x05,0x12,0x03,0x24,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01,0x12,0x03, -0x24,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, -0x01,0x03,0x12,0x03,0x24,0x11,0x12,0x0a,0x0a,0x0a, -0x02,0x04,0x06,0x12,0x04,0x27,0x00,0x2c,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03,0x27,0x08, -0x26,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x00,0x12, -0x03,0x28,0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x00,0x06,0x12,0x03,0x28,0x02,0x17,0x0a,0x0c, -0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12,0x03,0x28, -0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00, -0x03,0x12,0x03,0x28,0x25,0x26,0x0a,0x0b,0x0a,0x04, -0x04,0x06,0x02,0x01,0x12,0x03,0x29,0x02,0x28,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x06,0x12,0x03, -0x29,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, -0x01,0x01,0x12,0x03,0x29,0x18,0x23,0x0a,0x0c,0x0a, -0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x29,0x26, -0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x02,0x12, -0x03,0x2a,0x02,0x26,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x02,0x06,0x12,0x03,0x2a,0x02,0x17,0x0a,0x0c, -0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03,0x2a, -0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02, -0x03,0x12,0x03,0x2a,0x24,0x25,0x0a,0x0b,0x0a,0x04, -0x04,0x06,0x02,0x03,0x12,0x03,0x2b,0x02,0x27,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x06,0x12,0x03, -0x2b,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, -0x03,0x01,0x12,0x03,0x2b,0x18,0x22,0x0a,0x0c,0x0a, -0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03,0x2b,0x25, -0x26,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12,0x04,0x2e, -0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x07,0x01, -0x12,0x03,0x2e,0x08,0x2a,0x0a,0x0b,0x0a,0x04,0x04, -0x07,0x02,0x00,0x12,0x03,0x2f,0x02,0x18,0x0a,0x0c, -0x0a,0x05,0x04,0x07,0x02,0x00,0x05,0x12,0x03,0x2f, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00, -0x01,0x12,0x03,0x2f,0x09,0x13,0x0a,0x0c,0x0a,0x05, -0x04,0x07,0x02,0x00,0x03,0x12,0x03,0x2f,0x16,0x17, -0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x01,0x12,0x03, -0x30,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, -0x01,0x05,0x12,0x03,0x30,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x07,0x02,0x01,0x01,0x12,0x03,0x30,0x09, -0x14,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x03, -0x12,0x03,0x30,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04, -0x07,0x02,0x02,0x12,0x03,0x31,0x02,0x17,0x0a,0x0c, -0x0a,0x05,0x04,0x07,0x02,0x02,0x05,0x12,0x03,0x31, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02, -0x01,0x12,0x03,0x31,0x09,0x12,0x0a,0x0c,0x0a,0x05, -0x04,0x07,0x02,0x02,0x03,0x12,0x03,0x31,0x15,0x16, -0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x03,0x12,0x03, -0x32,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, -0x03,0x05,0x12,0x03,0x32,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x07,0x02,0x03,0x01,0x12,0x03,0x32,0x09, -0x13,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x03, -0x12,0x03,0x32,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04, -0x08,0x12,0x04,0x35,0x00,0x3a,0x01,0x0a,0x0a,0x0a, -0x03,0x04,0x08,0x01,0x12,0x03,0x35,0x08,0x27,0x0a, -0x0b,0x0a,0x04,0x04,0x08,0x02,0x00,0x12,0x03,0x36, -0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00, -0x05,0x12,0x03,0x36,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x08,0x02,0x00,0x01,0x12,0x03,0x36,0x09,0x13, -0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x03,0x12, -0x03,0x36,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x08, -0x02,0x01,0x12,0x03,0x37,0x02,0x19,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x01,0x05,0x12,0x03,0x37,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x01, -0x12,0x03,0x37,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04, -0x08,0x02,0x01,0x03,0x12,0x03,0x37,0x17,0x18,0x0a, -0x0b,0x0a,0x04,0x04,0x08,0x02,0x02,0x12,0x03,0x38, -0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02, -0x05,0x12,0x03,0x38,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x08,0x02,0x02,0x01,0x12,0x03,0x38,0x09,0x12, -0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x03,0x12, -0x03,0x38,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x08, -0x02,0x03,0x12,0x03,0x39,0x02,0x18,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x03,0x05,0x12,0x03,0x39,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x01, -0x12,0x03,0x39,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04, -0x08,0x02,0x03,0x03,0x12,0x03,0x39,0x16,0x17,0x0a, -0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,0x3c,0x00,0x41, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01,0x12,0x03, -0x3c,0x08,0x2e,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02, -0x00,0x12,0x03,0x3d,0x02,0x18,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x00,0x05,0x12,0x03,0x3d,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x01,0x12, -0x03,0x3d,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09, -0x02,0x00,0x03,0x12,0x03,0x3d,0x16,0x17,0x0a,0x0b, -0x0a,0x04,0x04,0x09,0x02,0x01,0x12,0x03,0x3e,0x02, -0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x05, -0x12,0x03,0x3e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x09,0x02,0x01,0x01,0x12,0x03,0x3e,0x09,0x14,0x0a, -0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x03,0x12,0x03, -0x3e,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02, -0x02,0x12,0x03,0x3f,0x02,0x17,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x02,0x05,0x12,0x03,0x3f,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x01,0x12, -0x03,0x3f,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x09, -0x02,0x02,0x03,0x12,0x03,0x3f,0x15,0x16,0x0a,0x0b, -0x0a,0x04,0x04,0x09,0x02,0x03,0x12,0x03,0x40,0x02, -0x18,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x05, -0x12,0x03,0x40,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x09,0x02,0x03,0x01,0x12,0x03,0x40,0x09,0x13,0x0a, -0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x03,0x12,0x03, -0x40,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x0a,0x12, -0x04,0x43,0x00,0x45,0x01,0x0a,0x0a,0x0a,0x03,0x04, -0x0a,0x01,0x12,0x03,0x43,0x08,0x25,0x0a,0x0b,0x0a, -0x04,0x04,0x0a,0x02,0x00,0x12,0x03,0x44,0x02,0x2d, -0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x04,0x12, -0x03,0x44,0x02,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x0a, -0x02,0x00,0x06,0x12,0x03,0x44,0x0b,0x20,0x0a,0x0c, -0x0a,0x05,0x04,0x0a,0x02,0x00,0x01,0x12,0x03,0x44, -0x21,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00, -0x03,0x12,0x03,0x44,0x2b,0x2c,0x0a,0x0a,0x0a,0x02, -0x04,0x0b,0x12,0x04,0x47,0x00,0x4a,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x0b,0x01,0x12,0x03,0x47,0x08,0x24, -0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02,0x00,0x12,0x03, -0x48,0x02,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02, -0x00,0x05,0x12,0x03,0x48,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x0b,0x02,0x00,0x01,0x12,0x03,0x48,0x09, -0x11,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x03, -0x12,0x03,0x48,0x14,0x15,0x0a,0x0b,0x0a,0x04,0x04, -0x0b,0x02,0x01,0x12,0x03,0x49,0x02,0x1f,0x0a,0x0c, -0x0a,0x05,0x04,0x0b,0x02,0x01,0x06,0x12,0x03,0x49, -0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01, -0x01,0x12,0x03,0x49,0x15,0x1a,0x0a,0x0c,0x0a,0x05, -0x04,0x0b,0x02,0x01,0x03,0x12,0x03,0x49,0x1d,0x1e, -0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12,0x04,0x4c,0x00, -0x4f,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0c,0x01,0x12, -0x03,0x4c,0x08,0x21,0x0a,0x0b,0x0a,0x04,0x04,0x0c, -0x02,0x00,0x12,0x03,0x4d,0x02,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x0c,0x02,0x00,0x05,0x12,0x03,0x4d,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x01, -0x12,0x03,0x4d,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, -0x0c,0x02,0x00,0x03,0x12,0x03,0x4d,0x11,0x12,0x0a, -0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01,0x12,0x03,0x4e, -0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01, -0x06,0x12,0x03,0x4e,0x02,0x14,0x0a,0x0c,0x0a,0x05, -0x04,0x0c,0x02,0x01,0x01,0x12,0x03,0x4e,0x15,0x1a, -0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x03,0x12, -0x03,0x4e,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0d, -0x12,0x04,0x51,0x00,0x54,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x0d,0x01,0x12,0x03,0x51,0x08,0x29,0x0a,0x0b, -0x0a,0x04,0x04,0x0d,0x02,0x00,0x12,0x03,0x52,0x02, -0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x05, -0x12,0x03,0x52,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x0d,0x02,0x00,0x01,0x12,0x03,0x52,0x09,0x15,0x0a, -0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x03,0x12,0x03, -0x52,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02, -0x01,0x12,0x03,0x53,0x02,0x1f,0x0a,0x0c,0x0a,0x05, -0x04,0x0d,0x02,0x01,0x06,0x12,0x03,0x53,0x02,0x14, -0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x01,0x12, -0x03,0x53,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d, -0x02,0x01,0x03,0x12,0x03,0x53,0x1d,0x1e,0x62,0x06, -0x70,0x72,0x6f,0x74,0x6f,0x33, +0x12,0x22,0x0a,0x0c,0x61,0x63,0x63,0x65,0x6c,0x65, +0x72,0x61,0x74,0x69,0x6f,0x6e,0x18,0x01,0x20,0x01, +0x28,0x01,0x52,0x0c,0x61,0x63,0x63,0x65,0x6c,0x65, +0x72,0x61,0x74,0x69,0x6f,0x6e,0x12,0x33,0x0a,0x05, +0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01,0x28, +0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72, +0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62, +0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f,0x6e, +0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65,0x42, +0x17,0x0a,0x15,0x6f,0x72,0x67,0x2e,0x77,0x70,0x69, +0x6c,0x69,0x62,0x2e,0x6d,0x61,0x74,0x68,0x2e,0x70, +0x72,0x6f,0x74,0x6f,0x4a,0xd6,0x12,0x0a,0x06,0x12, +0x04,0x00,0x00,0x54,0x01,0x0a,0x08,0x0a,0x01,0x0c, +0x12,0x03,0x00,0x00,0x12,0x0a,0x08,0x0a,0x01,0x02, +0x12,0x03,0x02,0x00,0x12,0x0a,0x09,0x0a,0x02,0x03, +0x00,0x12,0x03,0x04,0x00,0x1a,0x0a,0x08,0x0a,0x01, +0x08,0x12,0x03,0x06,0x00,0x2e,0x0a,0x09,0x0a,0x02, +0x08,0x01,0x12,0x03,0x06,0x00,0x2e,0x0a,0x0a,0x0a, +0x02,0x04,0x00,0x12,0x04,0x08,0x00,0x0c,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x00,0x01,0x12,0x03,0x08,0x08, +0x1d,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x00,0x12, +0x03,0x09,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00, +0x02,0x00,0x05,0x12,0x03,0x09,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x00,0x01,0x12,0x03,0x09, +0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00, +0x03,0x12,0x03,0x09,0x0e,0x0f,0x0a,0x0b,0x0a,0x04, +0x04,0x00,0x02,0x01,0x12,0x03,0x0a,0x02,0x10,0x0a, +0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x05,0x12,0x03, +0x0a,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, +0x01,0x01,0x12,0x03,0x0a,0x09,0x0b,0x0a,0x0c,0x0a, +0x05,0x04,0x00,0x02,0x01,0x03,0x12,0x03,0x0a,0x0e, +0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x02,0x12, +0x03,0x0b,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x00, +0x02,0x02,0x05,0x12,0x03,0x0b,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x02,0x01,0x12,0x03,0x0b, +0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02, +0x03,0x12,0x03,0x0b,0x11,0x12,0x0a,0x0a,0x0a,0x02, +0x04,0x01,0x12,0x04,0x0e,0x00,0x12,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x01,0x01,0x12,0x03,0x0e,0x08,0x24, +0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x00,0x12,0x03, +0x0f,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, +0x00,0x05,0x12,0x03,0x0f,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x00,0x01,0x12,0x03,0x0f,0x09, +0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x03, +0x12,0x03,0x0f,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04, +0x01,0x02,0x01,0x12,0x03,0x10,0x02,0x10,0x0a,0x0c, +0x0a,0x05,0x04,0x01,0x02,0x01,0x05,0x12,0x03,0x10, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01, +0x01,0x12,0x03,0x10,0x09,0x0b,0x0a,0x0c,0x0a,0x05, +0x04,0x01,0x02,0x01,0x03,0x12,0x03,0x10,0x0e,0x0f, +0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x02,0x12,0x03, +0x11,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, +0x02,0x05,0x12,0x03,0x11,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x02,0x01,0x12,0x03,0x11,0x09, +0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x03, +0x12,0x03,0x11,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04, +0x02,0x12,0x04,0x14,0x00,0x16,0x01,0x0a,0x0a,0x0a, +0x03,0x04,0x02,0x01,0x12,0x03,0x14,0x08,0x2b,0x0a, +0x0b,0x0a,0x04,0x04,0x02,0x02,0x00,0x12,0x03,0x15, +0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00, +0x05,0x12,0x03,0x15,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x02,0x02,0x00,0x01,0x12,0x03,0x15,0x09,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x03,0x12, +0x03,0x15,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x03, +0x12,0x04,0x18,0x00,0x1b,0x01,0x0a,0x0a,0x0a,0x03, +0x04,0x03,0x01,0x12,0x03,0x18,0x08,0x2c,0x0a,0x0b, +0x0a,0x04,0x04,0x03,0x02,0x00,0x12,0x03,0x19,0x02, +0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x05, +0x12,0x03,0x19,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x03,0x02,0x00,0x01,0x12,0x03,0x19,0x09,0x0d,0x0a, +0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x03,0x12,0x03, +0x19,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02, +0x01,0x12,0x03,0x1a,0x02,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x03,0x02,0x01,0x05,0x12,0x03,0x1a,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x01,0x12, +0x03,0x1a,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x03, +0x02,0x01,0x03,0x12,0x03,0x1a,0x11,0x12,0x0a,0x0a, +0x0a,0x02,0x04,0x04,0x12,0x04,0x1d,0x00,0x20,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01,0x12,0x03,0x1d, +0x08,0x33,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x00, +0x12,0x03,0x1e,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04, +0x04,0x02,0x00,0x05,0x12,0x03,0x1e,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x01,0x12,0x03, +0x1e,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, +0x00,0x03,0x12,0x03,0x1e,0x10,0x11,0x0a,0x0b,0x0a, +0x04,0x04,0x04,0x02,0x01,0x12,0x03,0x1f,0x02,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x05,0x12, +0x03,0x1f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04, +0x02,0x01,0x01,0x12,0x03,0x1f,0x09,0x0e,0x0a,0x0c, +0x0a,0x05,0x04,0x04,0x02,0x01,0x03,0x12,0x03,0x1f, +0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x05,0x12,0x04, +0x22,0x00,0x25,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x05, +0x01,0x12,0x03,0x22,0x08,0x2f,0x0a,0x0b,0x0a,0x04, +0x04,0x05,0x02,0x00,0x12,0x03,0x23,0x02,0x12,0x0a, +0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x05,0x12,0x03, +0x23,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, +0x00,0x01,0x12,0x03,0x23,0x09,0x0d,0x0a,0x0c,0x0a, +0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03,0x23,0x10, +0x11,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x01,0x12, +0x03,0x24,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x05, +0x02,0x01,0x05,0x12,0x03,0x24,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x05,0x02,0x01,0x01,0x12,0x03,0x24, +0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01, +0x03,0x12,0x03,0x24,0x11,0x12,0x0a,0x0a,0x0a,0x02, +0x04,0x06,0x12,0x04,0x27,0x00,0x2c,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x06,0x01,0x12,0x03,0x27,0x08,0x26, +0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x00,0x12,0x03, +0x28,0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x00,0x06,0x12,0x03,0x28,0x02,0x17,0x0a,0x0c,0x0a, +0x05,0x04,0x06,0x02,0x00,0x01,0x12,0x03,0x28,0x18, +0x22,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x03, +0x12,0x03,0x28,0x25,0x26,0x0a,0x0b,0x0a,0x04,0x04, +0x06,0x02,0x01,0x12,0x03,0x29,0x02,0x28,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x01,0x06,0x12,0x03,0x29, +0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01, +0x01,0x12,0x03,0x29,0x18,0x23,0x0a,0x0c,0x0a,0x05, +0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x29,0x26,0x27, +0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x02,0x12,0x03, +0x2a,0x02,0x26,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x02,0x06,0x12,0x03,0x2a,0x02,0x17,0x0a,0x0c,0x0a, +0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03,0x2a,0x18, +0x21,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x03, +0x12,0x03,0x2a,0x24,0x25,0x0a,0x0b,0x0a,0x04,0x04, +0x06,0x02,0x03,0x12,0x03,0x2b,0x02,0x27,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x03,0x06,0x12,0x03,0x2b, +0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03, +0x01,0x12,0x03,0x2b,0x18,0x22,0x0a,0x0c,0x0a,0x05, +0x04,0x06,0x02,0x03,0x03,0x12,0x03,0x2b,0x25,0x26, +0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12,0x04,0x2e,0x00, +0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x07,0x01,0x12, +0x03,0x2e,0x08,0x2a,0x0a,0x0b,0x0a,0x04,0x04,0x07, +0x02,0x00,0x12,0x03,0x2f,0x02,0x18,0x0a,0x0c,0x0a, +0x05,0x04,0x07,0x02,0x00,0x05,0x12,0x03,0x2f,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x01, +0x12,0x03,0x2f,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04, +0x07,0x02,0x00,0x03,0x12,0x03,0x2f,0x16,0x17,0x0a, +0x0b,0x0a,0x04,0x04,0x07,0x02,0x01,0x12,0x03,0x30, +0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01, +0x05,0x12,0x03,0x30,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x07,0x02,0x01,0x01,0x12,0x03,0x30,0x09,0x14, +0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x01,0x03,0x12, +0x03,0x30,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x07, +0x02,0x02,0x12,0x03,0x31,0x02,0x17,0x0a,0x0c,0x0a, +0x05,0x04,0x07,0x02,0x02,0x05,0x12,0x03,0x31,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x02,0x01, +0x12,0x03,0x31,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04, +0x07,0x02,0x02,0x03,0x12,0x03,0x31,0x15,0x16,0x0a, +0x0b,0x0a,0x04,0x04,0x07,0x02,0x03,0x12,0x03,0x32, +0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03, +0x05,0x12,0x03,0x32,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x07,0x02,0x03,0x01,0x12,0x03,0x32,0x09,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x03,0x03,0x12, +0x03,0x32,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x08, +0x12,0x04,0x35,0x00,0x3a,0x01,0x0a,0x0a,0x0a,0x03, +0x04,0x08,0x01,0x12,0x03,0x35,0x08,0x27,0x0a,0x0b, +0x0a,0x04,0x04,0x08,0x02,0x00,0x12,0x03,0x36,0x02, +0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x05, +0x12,0x03,0x36,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x00,0x01,0x12,0x03,0x36,0x09,0x13,0x0a, +0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x03,0x12,0x03, +0x36,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02, +0x01,0x12,0x03,0x37,0x02,0x19,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x01,0x05,0x12,0x03,0x37,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x01,0x12, +0x03,0x37,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x08, +0x02,0x01,0x03,0x12,0x03,0x37,0x17,0x18,0x0a,0x0b, +0x0a,0x04,0x04,0x08,0x02,0x02,0x12,0x03,0x38,0x02, +0x17,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x05, +0x12,0x03,0x38,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x02,0x01,0x12,0x03,0x38,0x09,0x12,0x0a, +0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x03,0x12,0x03, +0x38,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02, +0x03,0x12,0x03,0x39,0x02,0x18,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x03,0x05,0x12,0x03,0x39,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x01,0x12, +0x03,0x39,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x08, +0x02,0x03,0x03,0x12,0x03,0x39,0x16,0x17,0x0a,0x0a, +0x0a,0x02,0x04,0x09,0x12,0x04,0x3c,0x00,0x41,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01,0x12,0x03,0x3c, +0x08,0x2e,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x00, +0x12,0x03,0x3d,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x00,0x05,0x12,0x03,0x3d,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x01,0x12,0x03, +0x3d,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, +0x00,0x03,0x12,0x03,0x3d,0x16,0x17,0x0a,0x0b,0x0a, +0x04,0x04,0x09,0x02,0x01,0x12,0x03,0x3e,0x02,0x19, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x05,0x12, +0x03,0x3e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x01,0x01,0x12,0x03,0x3e,0x09,0x14,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x01,0x03,0x12,0x03,0x3e, +0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x02, +0x12,0x03,0x3f,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x02,0x05,0x12,0x03,0x3f,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x01,0x12,0x03, +0x3f,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, +0x02,0x03,0x12,0x03,0x3f,0x15,0x16,0x0a,0x0b,0x0a, +0x04,0x04,0x09,0x02,0x03,0x12,0x03,0x40,0x02,0x18, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x05,0x12, +0x03,0x40,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x03,0x01,0x12,0x03,0x40,0x09,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x03,0x03,0x12,0x03,0x40, +0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x0a,0x12,0x04, +0x43,0x00,0x45,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0a, +0x01,0x12,0x03,0x43,0x08,0x25,0x0a,0x0b,0x0a,0x04, +0x04,0x0a,0x02,0x00,0x12,0x03,0x44,0x02,0x2d,0x0a, +0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x04,0x12,0x03, +0x44,0x02,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02, +0x00,0x06,0x12,0x03,0x44,0x0b,0x20,0x0a,0x0c,0x0a, +0x05,0x04,0x0a,0x02,0x00,0x01,0x12,0x03,0x44,0x21, +0x28,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x03, +0x12,0x03,0x44,0x2b,0x2c,0x0a,0x0a,0x0a,0x02,0x04, +0x0b,0x12,0x04,0x47,0x00,0x4a,0x01,0x0a,0x0a,0x0a, +0x03,0x04,0x0b,0x01,0x12,0x03,0x47,0x08,0x24,0x0a, +0x0b,0x0a,0x04,0x04,0x0b,0x02,0x00,0x12,0x03,0x48, +0x02,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00, +0x05,0x12,0x03,0x48,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x0b,0x02,0x00,0x01,0x12,0x03,0x48,0x09,0x11, +0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x03,0x12, +0x03,0x48,0x14,0x15,0x0a,0x0b,0x0a,0x04,0x04,0x0b, +0x02,0x01,0x12,0x03,0x49,0x02,0x1f,0x0a,0x0c,0x0a, +0x05,0x04,0x0b,0x02,0x01,0x06,0x12,0x03,0x49,0x02, +0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x01,0x01, +0x12,0x03,0x49,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04, +0x0b,0x02,0x01,0x03,0x12,0x03,0x49,0x1d,0x1e,0x0a, +0x0a,0x0a,0x02,0x04,0x0c,0x12,0x04,0x4c,0x00,0x4f, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0c,0x01,0x12,0x03, +0x4c,0x08,0x21,0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02, +0x00,0x12,0x03,0x4d,0x02,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x0c,0x02,0x00,0x05,0x12,0x03,0x4d,0x02,0x08, +0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x01,0x12, +0x03,0x4d,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x0c, +0x02,0x00,0x03,0x12,0x03,0x4d,0x11,0x12,0x0a,0x0b, +0x0a,0x04,0x04,0x0c,0x02,0x01,0x12,0x03,0x4e,0x02, +0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x06, +0x12,0x03,0x4e,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04, +0x0c,0x02,0x01,0x01,0x12,0x03,0x4e,0x15,0x1a,0x0a, +0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x03,0x12,0x03, +0x4e,0x1d,0x1e,0x0a,0x0a,0x0a,0x02,0x04,0x0d,0x12, +0x04,0x51,0x00,0x54,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x0d,0x01,0x12,0x03,0x51,0x08,0x28,0x0a,0x0b,0x0a, +0x04,0x04,0x0d,0x02,0x00,0x12,0x03,0x52,0x02,0x1a, +0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x05,0x12, +0x03,0x52,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x0d, +0x02,0x00,0x01,0x12,0x03,0x52,0x09,0x15,0x0a,0x0c, +0x0a,0x05,0x04,0x0d,0x02,0x00,0x03,0x12,0x03,0x52, +0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02,0x01, +0x12,0x03,0x53,0x02,0x1f,0x0a,0x0c,0x0a,0x05,0x04, +0x0d,0x02,0x01,0x06,0x12,0x03,0x53,0x02,0x14,0x0a, +0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x01,0x12,0x03, +0x53,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02, +0x01,0x03,0x12,0x03,0x53,0x1d,0x1e,0x62,0x06,0x70, +0x72,0x6f,0x74,0x6f,0x33, }; static const char file_name[] = "kinematics.proto"; static const char wpi_proto_ProtobufChassisSpeeds_name[] = "wpi.proto.ProtobufChassisSpeeds"; @@ -511,10 +511,10 @@ pb_filedesc_t wpi_proto_ProtobufSwerveModuleState::file_descriptor(void) noexcep PB_BIND(wpi_proto_ProtobufSwerveModuleState, wpi_proto_ProtobufSwerveModuleState, AUTO) -static const char wpi_proto_ProtobufSwerveModuleAccelerations_name[] = "wpi.proto.ProtobufSwerveModuleAccelerations"; -std::string_view wpi_proto_ProtobufSwerveModuleAccelerations::msg_name(void) noexcept { return wpi_proto_ProtobufSwerveModuleAccelerations_name; } -pb_filedesc_t wpi_proto_ProtobufSwerveModuleAccelerations::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } -PB_BIND(wpi_proto_ProtobufSwerveModuleAccelerations, wpi_proto_ProtobufSwerveModuleAccelerations, AUTO) +static const char wpi_proto_ProtobufSwerveModuleAcceleration_name[] = "wpi.proto.ProtobufSwerveModuleAcceleration"; +std::string_view wpi_proto_ProtobufSwerveModuleAcceleration::msg_name(void) noexcept { return wpi_proto_ProtobufSwerveModuleAcceleration_name; } +pb_filedesc_t wpi_proto_ProtobufSwerveModuleAcceleration::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } +PB_BIND(wpi_proto_ProtobufSwerveModuleAcceleration, wpi_proto_ProtobufSwerveModuleAcceleration, AUTO) diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h index 80b176bea1d..f73aa9daf15 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h @@ -141,14 +141,14 @@ typedef struct _wpi_proto_ProtobufSwerveModuleState { pb_callback_t angle; } wpi_proto_ProtobufSwerveModuleState; -typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { +typedef struct _wpi_proto_ProtobufSwerveModuleAcceleration { static const pb_msgdesc_t* msg_descriptor(void) noexcept; static std::string_view msg_name(void) noexcept; static pb_filedesc_t file_descriptor(void) noexcept; double acceleration; pb_callback_t angle; -} wpi_proto_ProtobufSwerveModuleAccelerations; +} wpi_proto_ProtobufSwerveModuleAcceleration; /* Initializer values for message structs */ @@ -165,7 +165,7 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { #define wpi_proto_ProtobufSwerveDriveKinematics_init_default {{{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModulePosition_init_default {0, {{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModuleState_init_default {0, {{NULL}, NULL}} -#define wpi_proto_ProtobufSwerveModuleAccelerations_init_default {0, {{NULL}, NULL}} +#define wpi_proto_ProtobufSwerveModuleAcceleration_init_default {0, {{NULL}, NULL}} #define wpi_proto_ProtobufChassisSpeeds_init_zero {0, 0, 0} #define wpi_proto_ProtobufChassisAccelerations_init_zero {0, 0, 0} #define wpi_proto_ProtobufDifferentialDriveKinematics_init_zero {0} @@ -179,7 +179,7 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { #define wpi_proto_ProtobufSwerveDriveKinematics_init_zero {{{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModulePosition_init_zero {0, {{NULL}, NULL}} #define wpi_proto_ProtobufSwerveModuleState_init_zero {0, {{NULL}, NULL}} -#define wpi_proto_ProtobufSwerveModuleAccelerations_init_zero {0, {{NULL}, NULL}} +#define wpi_proto_ProtobufSwerveModuleAcceleration_init_zero {0, {{NULL}, NULL}} /* Field tags (for use in manual encoding/decoding) */ #define wpi_proto_ProtobufChassisSpeeds_vx_tag 1 @@ -216,8 +216,8 @@ typedef struct _wpi_proto_ProtobufSwerveModuleAccelerations { #define wpi_proto_ProtobufSwerveModulePosition_angle_tag 2 #define wpi_proto_ProtobufSwerveModuleState_speed_tag 1 #define wpi_proto_ProtobufSwerveModuleState_angle_tag 2 -#define wpi_proto_ProtobufSwerveModuleAccelerations_acceleration_tag 1 -#define wpi_proto_ProtobufSwerveModuleAccelerations_angle_tag 2 +#define wpi_proto_ProtobufSwerveModuleAcceleration_acceleration_tag 1 +#define wpi_proto_ProtobufSwerveModuleAcceleration_angle_tag 2 /* Struct field encoding specification for nanopb */ #define wpi_proto_ProtobufChassisSpeeds_FIELDLIST(X, a) \ @@ -313,19 +313,19 @@ X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2) #define wpi_proto_ProtobufSwerveModuleState_DEFAULT NULL #define wpi_proto_ProtobufSwerveModuleState_angle_MSGTYPE wpi_proto_ProtobufRotation2d -#define wpi_proto_ProtobufSwerveModuleAccelerations_FIELDLIST(X, a) \ +#define wpi_proto_ProtobufSwerveModuleAcceleration_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, DOUBLE, acceleration, 1) \ X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2) -#define wpi_proto_ProtobufSwerveModuleAccelerations_CALLBACK pb_default_field_callback -#define wpi_proto_ProtobufSwerveModuleAccelerations_DEFAULT NULL -#define wpi_proto_ProtobufSwerveModuleAccelerations_angle_MSGTYPE wpi_proto_ProtobufRotation2d +#define wpi_proto_ProtobufSwerveModuleAcceleration_CALLBACK pb_default_field_callback +#define wpi_proto_ProtobufSwerveModuleAcceleration_DEFAULT NULL +#define wpi_proto_ProtobufSwerveModuleAcceleration_angle_MSGTYPE wpi_proto_ProtobufRotation2d /* Maximum encoded size of messages (where known) */ /* wpi_proto_ProtobufMecanumDriveKinematics_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveDriveKinematics_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveModulePosition_size depends on runtime parameters */ /* wpi_proto_ProtobufSwerveModuleState_size depends on runtime parameters */ -/* wpi_proto_ProtobufSwerveModuleAccelerations_size depends on runtime parameters */ +/* wpi_proto_ProtobufSwerveModuleAcceleration_size depends on runtime parameters */ #define WPI_PROTO_KINEMATICS_NPB_H_MAX_SIZE wpi_proto_ProtobufMecanumDriveWheelPositions_size #define wpi_proto_ProtobufChassisAccelerations_size 27 #define wpi_proto_ProtobufChassisSpeeds_size 27 diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java index 56b441348db..9aa7eca35b9 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java @@ -43,7 +43,7 @@ */ @SuppressWarnings("overrides") public class SwerveDriveKinematics - implements Kinematics, + implements Kinematics, ProtobufSerializable, StructSerializable { private final SimpleMatrix m_firstOrderInverseKinematics; @@ -455,7 +455,7 @@ public static final SwerveDriveKinematicsStruct getStruct(int numModules) { * component, the robot will rotate around that corner. * @return An array containing the module accelerations. */ - public SwerveModuleAccelerations[] toSwerveModuleAccelerations( + public SwerveModuleAcceleration[] toSwerveModuleAccelerations( ChassisAccelerations chassisAccelerations, double angularVelocity, Translation2d centerOfRotation) { @@ -463,13 +463,13 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( // by FRC Team 449 - The Blair Robot Project, Rafi Pedersen // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf - var moduleAccelerations = new SwerveModuleAccelerations[m_numModules]; + var moduleAccelerations = new SwerveModuleAcceleration[m_numModules]; if (chassisAccelerations.ax == 0.0 && chassisAccelerations.ay == 0.0 && chassisAccelerations.alpha == 0.0) { for (int i = 0; i < m_numModules; i++) { - moduleAccelerations[i] = new SwerveModuleAccelerations(0.0, Rotation2d.kZero); + moduleAccelerations[i] = new SwerveModuleAcceleration(0.0, Rotation2d.kZero); } return moduleAccelerations; } @@ -499,10 +499,10 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( if (linearAcceleration <= 1e-6) { moduleAccelerations[i] = - new SwerveModuleAccelerations(linearAcceleration, Rotation2d.kZero); + new SwerveModuleAcceleration(linearAcceleration, Rotation2d.kZero); } else { moduleAccelerations[i] = - new SwerveModuleAccelerations(linearAcceleration, new Rotation2d(x, y)); + new SwerveModuleAcceleration(linearAcceleration, new Rotation2d(x, y)); } } @@ -517,13 +517,13 @@ public SwerveModuleAccelerations[] toSwerveModuleAccelerations( * @param angularVelocity The desired robot angular velocity. * @return An array containing the module accelerations. */ - public SwerveModuleAccelerations[] toSwerveModuleAccelerations( + public SwerveModuleAcceleration[] toSwerveModuleAccelerations( ChassisAccelerations chassisAccelerations, double angularVelocity) { return toSwerveModuleAccelerations(chassisAccelerations, angularVelocity, Translation2d.kZero); } @Override - public SwerveModuleAccelerations[] toWheelAccelerations( + public SwerveModuleAcceleration[] toWheelAccelerations( ChassisAccelerations chassisAccelerations) { return toSwerveModuleAccelerations(chassisAccelerations, 0.0); } @@ -541,7 +541,7 @@ public SwerveModuleAccelerations[] toWheelAccelerations( */ @Override public ChassisAccelerations toChassisAccelerations( - SwerveModuleAccelerations... moduleAccelerations) { + SwerveModuleAcceleration... moduleAccelerations) { // Derivation for second-order kinematics from "Swerve Drive Second Order Kinematics" // by FRC Team 449 - The Blair Robot Project, Rafi Pedersen // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAccelerations.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java similarity index 78% rename from wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAccelerations.java rename to wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java index 6201942911f..ca177bddc67 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAccelerations.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java @@ -9,16 +9,16 @@ import java.util.Objects; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.interpolation.Interpolatable; -import org.wpilib.math.kinematics.proto.SwerveModuleAccelerationsProto; -import org.wpilib.math.kinematics.struct.SwerveModuleAccelerationsStruct; +import org.wpilib.math.kinematics.proto.SwerveModuleAccelerationProto; +import org.wpilib.math.kinematics.struct.SwerveModuleAccelerationStruct; import org.wpilib.units.measure.LinearAcceleration; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; /** Represents the accelerations of one swerve module. */ -public class SwerveModuleAccelerations - implements Interpolatable, - Comparable, +public class SwerveModuleAcceleration + implements Interpolatable, + Comparable, ProtobufSerializable, StructSerializable { /** Acceleration of the wheel of the module in meters per second squared. */ @@ -28,14 +28,14 @@ public class SwerveModuleAccelerations public Rotation2d angle = new Rotation2d(); /** SwerveModuleAccelerations protobuf for serialization. */ - public static final SwerveModuleAccelerationsProto proto = new SwerveModuleAccelerationsProto(); + public static final SwerveModuleAccelerationProto proto = new SwerveModuleAccelerationProto(); /** SwerveModuleAccelerations struct for serialization. */ - public static final SwerveModuleAccelerationsStruct struct = - new SwerveModuleAccelerationsStruct(); + public static final SwerveModuleAccelerationStruct struct = + new SwerveModuleAccelerationStruct(); /** Constructs a SwerveModuleAccelerations with zeros for acceleration and angle. */ - public SwerveModuleAccelerations() {} + public SwerveModuleAcceleration() {} /** * Constructs a SwerveModuleAccelerations. @@ -43,7 +43,7 @@ public SwerveModuleAccelerations() {} * @param acceleration The acceleration of the wheel of the module in meters per second squared. * @param angle The angle of the acceleration vector. */ - public SwerveModuleAccelerations(double acceleration, Rotation2d angle) { + public SwerveModuleAcceleration(double acceleration, Rotation2d angle) { this.acceleration = acceleration; this.angle = angle; } @@ -54,7 +54,7 @@ public SwerveModuleAccelerations(double acceleration, Rotation2d angle) { * @param acceleration The acceleration of the wheel of the module. * @param angle The angle of the acceleration vector. */ - public SwerveModuleAccelerations(LinearAcceleration acceleration, Rotation2d angle) { + public SwerveModuleAcceleration(LinearAcceleration acceleration, Rotation2d angle) { this(acceleration.in(MetersPerSecondPerSecond), angle); } @@ -66,18 +66,18 @@ public SwerveModuleAccelerations(LinearAcceleration acceleration, Rotation2d ang * @return The interpolated value. */ @Override - public SwerveModuleAccelerations interpolate(SwerveModuleAccelerations endValue, double t) { + public SwerveModuleAcceleration interpolate(SwerveModuleAcceleration endValue, double t) { // Clamp t to [0, 1] t = Math.max(0.0, Math.min(1.0, t)); - return new SwerveModuleAccelerations( + return new SwerveModuleAcceleration( acceleration + t * (endValue.acceleration - acceleration), angle.interpolate(endValue.angle, t)); } @Override public boolean equals(Object obj) { - return obj instanceof SwerveModuleAccelerations other + return obj instanceof SwerveModuleAcceleration other && Math.abs(other.acceleration - acceleration) < 1E-9 && angle.equals(other.angle); } @@ -95,7 +95,7 @@ public int hashCode() { * @return 1 if this is greater, 0 if both are equal, -1 if other is greater. */ @Override - public int compareTo(SwerveModuleAccelerations other) { + public int compareTo(SwerveModuleAcceleration other) { return Double.compare(this.acceleration, other.acceleration); } diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationsProto.java b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java similarity index 68% rename from wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationsProto.java rename to wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java index aa12ff5ab0d..fba4866ad0c 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationsProto.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java @@ -5,16 +5,16 @@ package org.wpilib.math.kinematics.proto; import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.kinematics.SwerveModuleAccelerations; +import org.wpilib.math.kinematics.SwerveModuleAcceleration; import org.wpilib.math.proto.Kinematics.ProtobufSwerveModuleAccelerations; import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; -public class SwerveModuleAccelerationsProto - implements Protobuf { +public class SwerveModuleAccelerationProto + implements Protobuf { @Override - public Class getTypeClass() { - return SwerveModuleAccelerations.class; + public Class getTypeClass() { + return SwerveModuleAcceleration.class; } @Override @@ -28,13 +28,13 @@ public ProtobufSwerveModuleAccelerations createMessage() { } @Override - public SwerveModuleAccelerations unpack(ProtobufSwerveModuleAccelerations msg) { - return new SwerveModuleAccelerations( + public SwerveModuleAcceleration unpack(ProtobufSwerveModuleAccelerations msg) { + return new SwerveModuleAcceleration( msg.getAcceleration(), Rotation2d.proto.unpack(msg.getAngle())); } @Override - public void pack(ProtobufSwerveModuleAccelerations msg, SwerveModuleAccelerations value) { + public void pack(ProtobufSwerveModuleAccelerations msg, SwerveModuleAcceleration value) { msg.setAcceleration(value.acceleration); Rotation2d.proto.pack(msg.getAngle(), value.angle); } diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationsStruct.java b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationStruct.java similarity index 68% rename from wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationsStruct.java rename to wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationStruct.java index 08e7fa89823..aeeb4a75244 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationsStruct.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationStruct.java @@ -6,13 +6,13 @@ import java.nio.ByteBuffer; import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.kinematics.SwerveModuleAccelerations; +import org.wpilib.math.kinematics.SwerveModuleAcceleration; import org.wpilib.util.struct.Struct; -public class SwerveModuleAccelerationsStruct implements Struct { +public class SwerveModuleAccelerationStruct implements Struct { @Override - public Class getTypeClass() { - return SwerveModuleAccelerations.class; + public Class getTypeClass() { + return SwerveModuleAcceleration.class; } @Override @@ -36,14 +36,14 @@ public Struct[] getNested() { } @Override - public SwerveModuleAccelerations unpack(ByteBuffer bb) { + public SwerveModuleAcceleration unpack(ByteBuffer bb) { double acceleration = bb.getDouble(); Rotation2d angle = Rotation2d.struct.unpack(bb); - return new SwerveModuleAccelerations(acceleration, angle); + return new SwerveModuleAcceleration(acceleration, angle); } @Override - public void pack(ByteBuffer bb, SwerveModuleAccelerations value) { + public void pack(ByteBuffer bb, SwerveModuleAcceleration value) { bb.putDouble(value.acceleration); Rotation2d.struct.pack(bb, value.angle); } diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp similarity index 73% rename from wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp rename to wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp index 197f0b1e99a..8596e8aea6c 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp @@ -2,13 +2,12 @@ // 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 "wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp" - +#include "wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp" #include "wpi/util/protobuf/ProtobufCallbacks.hpp" #include "wpimath/protobuf/kinematics.npb.h" -std::optional wpi::util::Protobuf< - wpi::math::SwerveModuleAccelerations>::Unpack(InputStream& stream) { +std::optional wpi::util::Protobuf< + wpi::math::SwerveModuleAcceleration>::Unpack(InputStream& stream) { wpi::util::UnpackCallback angle; wpi_proto_ProtobufSwerveModuleAccelerations msg{ .acceleration = 0, @@ -24,14 +23,14 @@ std::optional wpi::util::Protobuf< return {}; } - return wpi::math::SwerveModuleAccelerations{ + return wpi::math::SwerveModuleAcceleration{ units::meters_per_second_squared_t{msg.acceleration}, iangle[0], }; } -bool wpi::util::Protobuf::Pack( - OutputStream& stream, const wpi::math::SwerveModuleAccelerations& value) { +bool wpi::util::Protobuf::Pack( + OutputStream& stream, const wpi::math::SwerveModuleAcceleration& value) { wpi::util::PackCallback angle{&value.angle}; wpi_proto_ProtobufSwerveModuleAccelerations msg{ .acceleration = value.acceleration.value(), diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationStruct.cpp similarity index 66% rename from wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp rename to wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationStruct.cpp index d9d43156847..4fa076f8c7e 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationStruct.cpp @@ -2,25 +2,24 @@ // 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 "wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp" - -#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" +#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp" +#include "wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp" #include "wpi/util/struct/Struct.hpp" -wpi::math::SwerveModuleAccelerations -wpi::util::Struct::Unpack( +wpi::math::SwerveModuleAcceleration +wpi::util::Struct::Unpack( std::span data) { constexpr size_t kAccelerationOff = 0; constexpr size_t kAngleOff = kAccelerationOff + 8; - return wpi::math::SwerveModuleAccelerations{ + return wpi::math::SwerveModuleAcceleration{ units::meters_per_second_squared_t{ wpi::util::UnpackStruct(data)}, wpi::util::UnpackStruct(data)}; } -void wpi::util::Struct::Pack( +void wpi::util::Struct::Pack( std::span data, - const wpi::math::SwerveModuleAccelerations& value) { + const wpi::math::SwerveModuleAcceleration& value) { constexpr size_t kAccelerationOff = 0; constexpr size_t kAngleOff = kAccelerationOff + 8; wpi::util::PackStruct(data, value.acceleration.value()); diff --git a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp index 9ae0922bded..663f2e7ace4 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp @@ -33,7 +33,7 @@ class SwerveDrivePoseEstimator : public PoseEstimator< wpi::util::array, wpi::util::array, - wpi::util::array> { + wpi::util::array> { public: /** * Constructs a SwerveDrivePoseEstimator with default standard deviations diff --git a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp index a95b090902d..102ebc92999 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp @@ -37,7 +37,7 @@ class SwerveDrivePoseEstimator3d : public PoseEstimator3d< wpi::util::array, wpi::util::array, - wpi::util::array> { + wpi::util::array> { public: /** * Constructs a SwerveDrivePoseEstimator3d with default standard deviations diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp index bea1f0ca833..c24fdf26a3f 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp @@ -16,7 +16,7 @@ #include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpi/math/kinematics/ChassisSpeeds.hpp" #include "wpi/math/kinematics/Kinematics.hpp" -#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" +#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp" #include "wpi/math/kinematics/SwerveModulePosition.hpp" #include "wpi/math/kinematics/SwerveModuleState.hpp" #include "wpi/math/linalg/EigenCore.hpp" @@ -55,7 +55,7 @@ class SwerveDriveKinematics : public Kinematics< wpi::util::array, wpi::util::array, - wpi::util::array> { + wpi::util::array> { public: /** * Constructs a swerve drive kinematics object. This takes in a variable @@ -475,7 +475,7 @@ class SwerveDriveKinematics * that corner. * @return An array containing the module accelerations. */ - wpi::util::array + wpi::util::array ToSwerveModuleAccelerations( const ChassisAccelerations& chassisAccelerations, const units::radians_per_second_t angularVelocity = 0.0_rad_per_s, @@ -484,7 +484,7 @@ class SwerveDriveKinematics // Kinematics" by FRC Team 449 - The Blair Robot Project, Rafi Pedersen // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf - wpi::util::array moduleAccelerations( + wpi::util::array moduleAccelerations( wpi::util::empty_array); if (chassisAccelerations.ax == 0.0_mps_sq && @@ -539,7 +539,7 @@ class SwerveDriveKinematics * @param chassisAccelerations The desired chassis accelerations. * @return An array containing the module accelerations. */ - wpi::util::array ToWheelAccelerations( + wpi::util::array ToWheelAccelerations( const ChassisAccelerations& chassisAccelerations) const override { return ToSwerveModuleAccelerations(chassisAccelerations); } @@ -556,12 +556,12 @@ class SwerveDriveKinematics * @return The resulting chassis accelerations. */ template < - std::convertible_to... ModuleAccelerations> + std::convertible_to... ModuleAccelerations> requires(sizeof...(ModuleAccelerations) == NumModules) ChassisAccelerations ToChassisAccelerations( ModuleAccelerations&&... moduleAccelerations) const { return this->ToChassisAccelerations( - wpi::util::array{ + wpi::util::array{ moduleAccelerations...}); } @@ -577,7 +577,7 @@ class SwerveDriveKinematics * @return The resulting chassis accelerations. */ ChassisAccelerations ToChassisAccelerations( - const wpi::util::array& + const wpi::util::array& moduleAccelerations) const override { // Derivation for second-order kinematics from "Swerve Drive Second Order // Kinematics" by FRC Team 449 - The Blair Robot Project, Rafi Pedersen @@ -586,7 +586,7 @@ class SwerveDriveKinematics Matrixd moduleAccelerationsMatrix; for (size_t i = 0; i < NumModules; i++) { - SwerveModuleAccelerations module = moduleAccelerations[i]; + SwerveModuleAcceleration module = moduleAccelerations[i]; moduleAccelerationsMatrix(i * 2 + 0, 0) = module.acceleration.value() * module.angle.Cos(); diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp index ae0b935580c..b6d9d1db637 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp @@ -31,7 +31,7 @@ template class SwerveDriveOdometry : public Odometry, wpi::util::array, - wpi::util::array> { + wpi::util::array> { public: /** * Constructs a SwerveDriveOdometry object. diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp index d05cf3f1143..84910d7ba81 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp @@ -33,7 +33,7 @@ class SwerveDriveOdometry3d : public Odometry3d< wpi::util::array, wpi::util::array, - wpi::util::array> { + wpi::util::array> { public: /** * Constructs a SwerveDriveOdometry3d object. diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAcceleration.hpp similarity index 81% rename from wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp rename to wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAcceleration.hpp index 3fef5b8f11d..0f72d030ed9 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAccelerations.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAcceleration.hpp @@ -13,7 +13,7 @@ namespace wpi::math { /** * Represents the accelerations of one swerve module. */ -struct WPILIB_DLLEXPORT SwerveModuleAccelerations { +struct WPILIB_DLLEXPORT SwerveModuleAcceleration { /** * Acceleration of the wheel of the module. */ @@ -30,7 +30,7 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * @param other The other object. * @return Whether the two objects are equal. */ - constexpr bool operator==(const SwerveModuleAccelerations& other) const { + constexpr bool operator==(const SwerveModuleAcceleration& other) const { return units::math::abs(acceleration - other.acceleration) < 1E-9_mps_sq && angle == other.angle; } @@ -42,8 +42,8 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * @param other The SwerveModuleAccelerations to add. * @return The sum of the SwerveModuleAccelerations. */ - SwerveModuleAccelerations operator+( - const SwerveModuleAccelerations& other) const { + SwerveModuleAcceleration operator+( + const SwerveModuleAcceleration& other) const { // Convert to Cartesian coordinates, add, then convert back auto thisX = acceleration * angle.Cos(); auto thisY = acceleration * angle.Sin(); @@ -66,8 +66,8 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * @param other The SwerveModuleAccelerations to subtract. * @return The difference between the two SwerveModuleAccelerations. */ - SwerveModuleAccelerations operator-( - const SwerveModuleAccelerations& other) const { + SwerveModuleAcceleration operator-( + const SwerveModuleAcceleration& other) const { return *this + (-other); } @@ -77,7 +77,7 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * * @return The inverse of the current SwerveModuleAccelerations. */ - constexpr SwerveModuleAccelerations operator-() const { + constexpr SwerveModuleAcceleration operator-() const { return {acceleration, angle + Rotation2d{180_deg}}; } @@ -88,7 +88,7 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * @param scalar The scalar to multiply by. * @return The scaled SwerveModuleAccelerations. */ - constexpr SwerveModuleAccelerations operator*(double scalar) const { + constexpr SwerveModuleAcceleration operator*(double scalar) const { if (scalar < 0) { return {-scalar * acceleration, angle + Rotation2d{180_deg}}; } @@ -102,11 +102,11 @@ struct WPILIB_DLLEXPORT SwerveModuleAccelerations { * @param scalar The scalar to divide by. * @return The scaled SwerveModuleAccelerations. */ - constexpr SwerveModuleAccelerations operator/(double scalar) const { + constexpr SwerveModuleAcceleration operator/(double scalar) const { return operator*(1.0 / scalar); } }; } // namespace wpi::math -#include "wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp" -#include "wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp" +#include "wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp" +#include "wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp similarity index 70% rename from wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp rename to wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp index d57185a97b1..5388ef8859b 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationsProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp @@ -4,21 +4,21 @@ #pragma once -#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" +#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/protobuf/Protobuf.hpp" #include "wpimath/protobuf/kinematics.npb.h" template <> struct WPILIB_DLLEXPORT - wpi::util::Protobuf { + wpi::util::Protobuf { using MessageStruct = wpi_proto_ProtobufSwerveModuleAccelerations; using InputStream = - wpi::util::ProtoInputStream; + wpi::util::ProtoInputStream; using OutputStream = - wpi::util::ProtoOutputStream; - static std::optional Unpack( + wpi::util::ProtoOutputStream; + static std::optional Unpack( InputStream& stream); static bool Pack(OutputStream& stream, - const wpi::math::SwerveModuleAccelerations& value); + const wpi::math::SwerveModuleAcceleration& value); }; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp similarity index 78% rename from wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp rename to wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp index db6277abb38..3aee857fe82 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationsStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp @@ -4,13 +4,13 @@ #pragma once -#include "wpi/math/kinematics/SwerveModuleAccelerations.hpp" +#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/struct/Struct.hpp" template <> struct WPILIB_DLLEXPORT - wpi::util::Struct { + wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "SwerveModuleAccelerations"; } @@ -21,10 +21,10 @@ struct WPILIB_DLLEXPORT return "double acceleration;Rotation2d angle"; } - static wpi::math::SwerveModuleAccelerations Unpack( + static wpi::math::SwerveModuleAcceleration Unpack( std::span data); static void Pack(std::span data, - const wpi::math::SwerveModuleAccelerations& value); + const wpi::math::SwerveModuleAcceleration& value); static void ForEachNested( std::invocable auto fn) { wpi::util::ForEachStructSchema(fn); @@ -32,5 +32,5 @@ struct WPILIB_DLLEXPORT }; static_assert( - wpi::util::StructSerializable); -static_assert(wpi::util::HasNestedStruct); + wpi::util::StructSerializable); +static_assert(wpi::util::HasNestedStruct); diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto index 888cfcd3a61..5e5e20527c5 100644 --- a/wpimath/src/main/proto/kinematics.proto +++ b/wpimath/src/main/proto/kinematics.proto @@ -79,7 +79,7 @@ message ProtobufSwerveModuleState { ProtobufRotation2d angle = 2; } -message ProtobufSwerveModuleAccelerations { +message ProtobufSwerveModuleAcceleration { double acceleration = 1; ProtobufRotation2d angle = 2; } diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java index 9b05c660d83..984d9e51790 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java @@ -491,14 +491,14 @@ void testTurnInPlaceInverseAccelerations() { @Test void testTurnInPlaceForwardAccelerations() { - SwerveModuleAccelerations flAccel = - new SwerveModuleAccelerations(106.629, Rotation2d.fromDegrees(135)); - SwerveModuleAccelerations frAccel = - new SwerveModuleAccelerations(106.629, Rotation2d.fromDegrees(45)); - SwerveModuleAccelerations blAccel = - new SwerveModuleAccelerations(106.629, Rotation2d.fromDegrees(-135)); - SwerveModuleAccelerations brAccel = - new SwerveModuleAccelerations(106.629, Rotation2d.fromDegrees(-45)); + SwerveModuleAcceleration flAccel = + new SwerveModuleAcceleration(106.629, Rotation2d.fromDegrees(135)); + SwerveModuleAcceleration frAccel = + new SwerveModuleAcceleration(106.629, Rotation2d.fromDegrees(45)); + SwerveModuleAcceleration blAccel = + new SwerveModuleAcceleration(106.629, Rotation2d.fromDegrees(-135)); + SwerveModuleAcceleration brAccel = + new SwerveModuleAcceleration(106.629, Rotation2d.fromDegrees(-45)); var chassisAccelerations = m_kinematics.toChassisAccelerations(flAccel, frAccel, blAccel, brAccel); diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationTest.java similarity index 80% rename from wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationsTest.java rename to wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationTest.java index 19e7f10216f..8362efd0ad4 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationTest.java @@ -13,12 +13,12 @@ import org.junit.jupiter.api.Test; import org.wpilib.math.geometry.Rotation2d; -class SwerveModuleAccelerationsTest { +class SwerveModuleAccelerationTest { private static final double kEpsilon = 1E-9; @Test void testDefaultConstructor() { - var moduleAccelerations = new SwerveModuleAccelerations(); + var moduleAccelerations = new SwerveModuleAcceleration(); assertAll( () -> assertEquals(0.0, moduleAccelerations.acceleration, kEpsilon), @@ -27,7 +27,7 @@ void testDefaultConstructor() { @Test void testParameterizedConstructor() { - var moduleAccelerations = new SwerveModuleAccelerations(2.5, new Rotation2d(1.5)); + var moduleAccelerations = new SwerveModuleAcceleration(2.5, new Rotation2d(1.5)); assertAll( () -> assertEquals(2.5, moduleAccelerations.acceleration, kEpsilon), @@ -38,7 +38,7 @@ void testParameterizedConstructor() { void testMeasureConstructor() { var acceleration = MetersPerSecondPerSecond.of(3.0); var angle = new Rotation2d(2.0); - var moduleAccelerations = new SwerveModuleAccelerations(acceleration, angle); + var moduleAccelerations = new SwerveModuleAcceleration(acceleration, angle); assertAll( () -> assertEquals(3.0, moduleAccelerations.acceleration, kEpsilon), @@ -47,9 +47,9 @@ void testMeasureConstructor() { @Test void testEquals() { - var moduleAccelerations1 = new SwerveModuleAccelerations(2.0, new Rotation2d(1.5)); - var moduleAccelerations2 = new SwerveModuleAccelerations(2.0, new Rotation2d(1.5)); - var moduleAccelerations3 = new SwerveModuleAccelerations(2.1, new Rotation2d(1.5)); + var moduleAccelerations1 = new SwerveModuleAcceleration(2.0, new Rotation2d(1.5)); + var moduleAccelerations2 = new SwerveModuleAcceleration(2.0, new Rotation2d(1.5)); + var moduleAccelerations3 = new SwerveModuleAcceleration(2.1, new Rotation2d(1.5)); assertEquals(moduleAccelerations1, moduleAccelerations2); assertNotEquals(moduleAccelerations1, moduleAccelerations3); @@ -57,18 +57,18 @@ void testEquals() { @Test void testCompareTo() { - var slower = new SwerveModuleAccelerations(1.0, new Rotation2d(2.0)); - var faster = new SwerveModuleAccelerations(2.0, new Rotation2d(1.0)); + var slower = new SwerveModuleAcceleration(1.0, new Rotation2d(2.0)); + var faster = new SwerveModuleAcceleration(2.0, new Rotation2d(1.0)); assertTrue(slower.compareTo(faster) < 0); assertTrue(faster.compareTo(slower) > 0); - assertEquals(0, slower.compareTo(new SwerveModuleAccelerations(1.0, new Rotation2d(2.0)))); + assertEquals(0, slower.compareTo(new SwerveModuleAcceleration(1.0, new Rotation2d(2.0)))); } @Test void testInterpolate() { - final var start = new SwerveModuleAccelerations(1.0, new Rotation2d(0.0)); - final var end = new SwerveModuleAccelerations(5.0, new Rotation2d(Math.PI / 2)); + final var start = new SwerveModuleAcceleration(1.0, new Rotation2d(0.0)); + final var end = new SwerveModuleAcceleration(5.0, new Rotation2d(Math.PI / 2)); // Test interpolation at t=0 (should return start) final var atStart = start.interpolate(end, 0.0); diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index a9278835422..549eae5957e 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -397,10 +397,10 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) { } TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardAccelerations) { - SwerveModuleAccelerations flAccel{106.629_mps_sq, 135_deg}; - SwerveModuleAccelerations frAccel{106.629_mps_sq, 45_deg}; - SwerveModuleAccelerations blAccel{106.629_mps_sq, -135_deg}; - SwerveModuleAccelerations brAccel{106.629_mps_sq, -45_deg}; + SwerveModuleAcceleration flAccel{106.629_mps_sq, 135_deg}; + SwerveModuleAcceleration frAccel{106.629_mps_sq, 45_deg}; + SwerveModuleAcceleration blAccel{106.629_mps_sq, -135_deg}; + SwerveModuleAcceleration brAccel{106.629_mps_sq, -45_deg}; auto chassisAccelerations = m_kinematics.ToChassisAccelerations(flAccel, frAccel, blAccel, brAccel); diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp index 4a925e40311..a35c8e5dbea 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp @@ -2,12 +2,11 @@ // 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 "wpi/math/kinematics/SwerveModuleAccelerations.hpp" - #include #include +#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp" #include "wpi/units/acceleration.hpp" using namespace wpi::math; @@ -15,14 +14,14 @@ using namespace wpi::math; static constexpr double kEpsilon = 1E-9; TEST(SwerveModuleAccelerationsTest, DefaultConstructor) { - SwerveModuleAccelerations moduleAccelerations; + SwerveModuleAcceleration moduleAccelerations; EXPECT_NEAR(moduleAccelerations.acceleration.value(), 0.0, kEpsilon); EXPECT_NEAR(moduleAccelerations.angle.Radians().value(), 0.0, kEpsilon); } TEST(SwerveModuleAccelerationsTest, ParameterizedConstructor) { - SwerveModuleAccelerations moduleAccelerations{2.5_mps_sq, + SwerveModuleAcceleration moduleAccelerations{2.5_mps_sq, Rotation2d{1.5_rad}}; EXPECT_NEAR(moduleAccelerations.acceleration.value(), 2.5, kEpsilon); @@ -30,11 +29,11 @@ TEST(SwerveModuleAccelerationsTest, ParameterizedConstructor) { } TEST(SwerveModuleAccelerationsTest, Equals) { - SwerveModuleAccelerations moduleAccelerations1{2.0_mps_sq, + SwerveModuleAcceleration moduleAccelerations1{2.0_mps_sq, Rotation2d{1.5_rad}}; - SwerveModuleAccelerations moduleAccelerations2{2.0_mps_sq, + SwerveModuleAcceleration moduleAccelerations2{2.0_mps_sq, Rotation2d{1.5_rad}}; - SwerveModuleAccelerations moduleAccelerations3{2.1_mps_sq, + SwerveModuleAcceleration moduleAccelerations3{2.1_mps_sq, Rotation2d{1.5_rad}}; EXPECT_EQ(moduleAccelerations1, moduleAccelerations2); From 49d95e58f1170202f8d77536f3947186c05e82f0 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 25 Nov 2025 13:02:40 -0500 Subject: [PATCH 78/82] wpiformat/javaformat Signed-off-by: Zach Harel --- .../math/kinematics/SwerveDriveKinematics.java | 3 +-- .../math/kinematics/SwerveModuleAcceleration.java | 3 +-- .../proto/SwerveModuleAccelerationProto.java | 14 +++++++------- .../proto/SwerveModuleAccelerationProto.cpp | 1 + .../struct/SwerveModuleAccelerationStruct.cpp | 6 +++--- .../struct/SwerveModuleAccelerationStruct.hpp | 3 +-- .../kinematics/SwerveModuleAccelerationsTest.cpp | 9 ++++----- 7 files changed, 18 insertions(+), 21 deletions(-) diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java index 9aa7eca35b9..b1b8638f4de 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java @@ -498,8 +498,7 @@ public SwerveModuleAcceleration[] toSwerveModuleAccelerations( double linearAcceleration = Math.hypot(x, y); if (linearAcceleration <= 1e-6) { - moduleAccelerations[i] = - new SwerveModuleAcceleration(linearAcceleration, Rotation2d.kZero); + moduleAccelerations[i] = new SwerveModuleAcceleration(linearAcceleration, Rotation2d.kZero); } else { moduleAccelerations[i] = new SwerveModuleAcceleration(linearAcceleration, new Rotation2d(x, y)); diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java index ca177bddc67..26ff4427e06 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java @@ -31,8 +31,7 @@ public class SwerveModuleAcceleration public static final SwerveModuleAccelerationProto proto = new SwerveModuleAccelerationProto(); /** SwerveModuleAccelerations struct for serialization. */ - public static final SwerveModuleAccelerationStruct struct = - new SwerveModuleAccelerationStruct(); + public static final SwerveModuleAccelerationStruct struct = new SwerveModuleAccelerationStruct(); /** Constructs a SwerveModuleAccelerations with zeros for acceleration and angle. */ public SwerveModuleAcceleration() {} diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java index fba4866ad0c..020d21d035a 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java @@ -6,12 +6,12 @@ import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModuleAcceleration; -import org.wpilib.math.proto.Kinematics.ProtobufSwerveModuleAccelerations; +import org.wpilib.math.proto.Kinematics.ProtobufSwerveModuleAcceleration; import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; public class SwerveModuleAccelerationProto - implements Protobuf { + implements Protobuf { @Override public Class getTypeClass() { return SwerveModuleAcceleration.class; @@ -19,22 +19,22 @@ public Class getTypeClass() { @Override public Descriptor getDescriptor() { - return ProtobufSwerveModuleAccelerations.getDescriptor(); + return ProtobufSwerveModuleAcceleration.getDescriptor(); } @Override - public ProtobufSwerveModuleAccelerations createMessage() { - return ProtobufSwerveModuleAccelerations.newInstance(); + public ProtobufSwerveModuleAcceleration createMessage() { + return ProtobufSwerveModuleAcceleration.newInstance(); } @Override - public SwerveModuleAcceleration unpack(ProtobufSwerveModuleAccelerations msg) { + public SwerveModuleAcceleration unpack(ProtobufSwerveModuleAcceleration msg) { return new SwerveModuleAcceleration( msg.getAcceleration(), Rotation2d.proto.unpack(msg.getAngle())); } @Override - public void pack(ProtobufSwerveModuleAccelerations msg, SwerveModuleAcceleration value) { + public void pack(ProtobufSwerveModuleAcceleration msg, SwerveModuleAcceleration value) { msg.setAcceleration(value.acceleration); Rotation2d.proto.pack(msg.getAngle(), value.angle); } diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp index 8596e8aea6c..957b81b5483 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include "wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp" + #include "wpi/util/protobuf/ProtobufCallbacks.hpp" #include "wpimath/protobuf/kinematics.npb.h" diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationStruct.cpp index 4fa076f8c7e..e9a82bd055c 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationStruct.cpp @@ -2,8 +2,9 @@ // 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 "wpi/math/kinematics/SwerveModuleAcceleration.hpp" #include "wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp" + +#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp" #include "wpi/util/struct/Struct.hpp" wpi::math::SwerveModuleAcceleration @@ -18,8 +19,7 @@ wpi::util::Struct::Unpack( } void wpi::util::Struct::Pack( - std::span data, - const wpi::math::SwerveModuleAcceleration& value) { + std::span data, const wpi::math::SwerveModuleAcceleration& value) { constexpr size_t kAccelerationOff = 0; constexpr size_t kAngleOff = kAccelerationOff + 8; wpi::util::PackStruct(data, value.acceleration.value()); diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp index 3aee857fe82..8062e8710b0 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp @@ -9,8 +9,7 @@ #include "wpi/util/struct/Struct.hpp" template <> -struct WPILIB_DLLEXPORT - wpi::util::Struct { +struct WPILIB_DLLEXPORT wpi::util::Struct { static constexpr std::string_view GetTypeName() { return "SwerveModuleAccelerations"; } diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp index a35c8e5dbea..d324ddece21 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp @@ -21,8 +21,7 @@ TEST(SwerveModuleAccelerationsTest, DefaultConstructor) { } TEST(SwerveModuleAccelerationsTest, ParameterizedConstructor) { - SwerveModuleAcceleration moduleAccelerations{2.5_mps_sq, - Rotation2d{1.5_rad}}; + SwerveModuleAcceleration moduleAccelerations{2.5_mps_sq, Rotation2d{1.5_rad}}; EXPECT_NEAR(moduleAccelerations.acceleration.value(), 2.5, kEpsilon); EXPECT_NEAR(moduleAccelerations.angle.Radians().value(), 1.5, kEpsilon); @@ -30,11 +29,11 @@ TEST(SwerveModuleAccelerationsTest, ParameterizedConstructor) { TEST(SwerveModuleAccelerationsTest, Equals) { SwerveModuleAcceleration moduleAccelerations1{2.0_mps_sq, - Rotation2d{1.5_rad}}; + Rotation2d{1.5_rad}}; SwerveModuleAcceleration moduleAccelerations2{2.0_mps_sq, - Rotation2d{1.5_rad}}; + Rotation2d{1.5_rad}}; SwerveModuleAcceleration moduleAccelerations3{2.1_mps_sq, - Rotation2d{1.5_rad}}; + Rotation2d{1.5_rad}}; EXPECT_EQ(moduleAccelerations1, moduleAccelerations2); EXPECT_NE(moduleAccelerations1, moduleAccelerations3); From d7d59602b93ab8275acc69c9559e06b994609907 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Wed, 26 Nov 2025 17:10:59 -0500 Subject: [PATCH 79/82] Refactor kinematics template parameters for consistency across java/cpp/reality Signed-off-by: Zach Harel --- .../org/wpilib/math/estimator/PoseEstimator.java | 2 +- .../org/wpilib/math/estimator/PoseEstimator3d.java | 2 +- .../kinematics/DifferentialDriveKinematics.java | 4 ++-- .../org/wpilib/math/kinematics/Kinematics.java | 4 ++-- .../math/kinematics/MecanumDriveKinematics.java | 2 +- .../java/org/wpilib/math/kinematics/Odometry.java | 4 ++-- .../org/wpilib/math/kinematics/Odometry3d.java | 4 ++-- .../math/kinematics/SwerveDriveKinematics.java | 4 ++-- .../estimator/DifferentialDrivePoseEstimator.hpp | 4 ++-- .../estimator/DifferentialDrivePoseEstimator3d.hpp | 4 ++-- .../math/estimator/MecanumDrivePoseEstimator.hpp | 2 +- .../math/estimator/MecanumDrivePoseEstimator3d.hpp | 4 ++-- .../include/wpi/math/estimator/PoseEstimator.hpp | 8 ++++---- .../include/wpi/math/estimator/PoseEstimator3d.hpp | 8 ++++---- .../math/estimator/SwerveDrivePoseEstimator.hpp | 2 +- .../math/estimator/SwerveDrivePoseEstimator3d.hpp | 2 +- .../kinematics/DifferentialDriveKinematics.hpp | 4 ++-- .../math/kinematics/DifferentialDriveOdometry.hpp | 4 ++-- .../kinematics/DifferentialDriveOdometry3d.hpp | 4 ++-- .../include/wpi/math/kinematics/Kinematics.hpp | 2 +- .../wpi/math/kinematics/MecanumDriveKinematics.hpp | 2 +- .../wpi/math/kinematics/MecanumDriveOdometry.hpp | 2 +- .../wpi/math/kinematics/MecanumDriveOdometry3d.hpp | 2 +- .../include/wpi/math/kinematics/Odometry.hpp | 6 +++--- .../include/wpi/math/kinematics/Odometry3d.hpp | 6 +++--- .../wpi/math/kinematics/SwerveDriveKinematics.hpp | 2 +- .../wpi/math/kinematics/SwerveDriveOdometry.hpp | 4 ++-- .../wpi/math/kinematics/SwerveDriveOdometry3d.hpp | 2 +- .../proto/SwerveModuleAccelerationProto.hpp | 2 +- .../python/semiwrap/controls/PoseEstimator.yml | 14 +++++++------- .../python/semiwrap/controls/PoseEstimator3d.yml | 14 +++++++------- .../main/python/semiwrap/kinematics/Kinematics.yml | 14 +++++++------- .../main/python/semiwrap/kinematics/Odometry.yml | 14 +++++++------- .../main/python/semiwrap/kinematics/Odometry3d.yml | 14 +++++++------- 34 files changed, 86 insertions(+), 86 deletions(-) diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java index 23f8f6152ff..0e869411384 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator.java @@ -66,7 +66,7 @@ public class PoseEstimator { */ @SuppressWarnings("PMD.UnusedFormalParameter") public PoseEstimator( - Kinematics kinematics, + Kinematics kinematics, Odometry odometry, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java index 41ad96e1189..d9ab292d2a5 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java @@ -74,7 +74,7 @@ public class PoseEstimator3d { */ @SuppressWarnings("PMD.UnusedFormalParameter") public PoseEstimator3d( - Kinematics kinematics, + Kinematics kinematics, Odometry3d odometry, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveKinematics.java index 8de988dc2d0..9dad84bc18c 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveKinematics.java @@ -24,9 +24,9 @@ */ public class DifferentialDriveKinematics implements Kinematics< + DifferentialDriveWheelPositions, DifferentialDriveWheelSpeeds, - DifferentialDriveWheelAccelerations, - DifferentialDriveWheelPositions>, + DifferentialDriveWheelAccelerations>, ProtobufSerializable, StructSerializable { /** Differential drive trackwidth in meters. */ diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/Kinematics.java b/wpimath/src/main/java/org/wpilib/math/kinematics/Kinematics.java index cfcffee521e..624b6126b02 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/Kinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/Kinematics.java @@ -12,11 +12,11 @@ * chassis accelerations into wheel accelerations. Robot code should not use this directly- Instead, * use the particular type for your drivetrain (e.g., {@link DifferentialDriveKinematics}). * + * @param

The type of the wheel positions. * @param The type of the wheel speeds. * @param The type of the wheel accelerations. - * @param

The type of the wheel positions. */ -public interface Kinematics extends Interpolator

{ +public interface Kinematics extends Interpolator

{ /** * Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This * method is often used for odometry -- determining the robot's position on the field using data diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveKinematics.java index fa1babd227c..0d7151aae72 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveKinematics.java @@ -35,7 +35,7 @@ */ public class MecanumDriveKinematics implements Kinematics< - MecanumDriveWheelSpeeds, MecanumDriveWheelAccelerations, MecanumDriveWheelPositions>, + MecanumDriveWheelPositions, MecanumDriveWheelSpeeds, MecanumDriveWheelAccelerations>, ProtobufSerializable, StructSerializable { private final SimpleMatrix m_inverseKinematics; diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java index 5a414cccf09..79dcd243cf5 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java @@ -20,7 +20,7 @@ * @param Wheel positions type. */ public class Odometry { - private final Kinematics m_kinematics; + private final Kinematics m_kinematics; private Pose2d m_pose; private Rotation2d m_gyroOffset; @@ -36,7 +36,7 @@ public class Odometry { * @param initialPose The starting position of the robot on the field. */ public Odometry( - Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) { + Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) { m_kinematics = kinematics; m_pose = initialPose; m_gyroOffset = m_pose.getRotation().minus(gyroAngle); diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java index 78cf18cadc5..dd68b4d7d3e 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java @@ -29,7 +29,7 @@ * @param Wheel positions type. */ public class Odometry3d { - private final Kinematics m_kinematics; + private final Kinematics m_kinematics; private Pose3d m_pose; private Rotation3d m_gyroOffset; @@ -45,7 +45,7 @@ public class Odometry3d { * @param initialPose The starting position of the robot on the field. */ public Odometry3d( - Kinematics kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) { + Kinematics kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) { m_kinematics = kinematics; m_pose = initialPose; m_gyroOffset = m_pose.getRotation().minus(gyroAngle); diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java index b1b8638f4de..35bbf99a360 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java @@ -43,7 +43,7 @@ */ @SuppressWarnings("overrides") public class SwerveDriveKinematics - implements Kinematics, + implements Kinematics, ProtobufSerializable, StructSerializable { private final SimpleMatrix m_firstOrderInverseKinematics; @@ -225,7 +225,7 @@ public Twist2d toTwist2d(SwerveModulePosition... moduleDeltas) { for (int i = 0; i < m_numModules; i++) { var module = moduleDeltas[i]; moduleDeltaMatrix.set(i * 2, 0, module.distance * module.angle.getCos()); - moduleDeltaMatrix.set(i * 2 + 1, module.distance * module.angle.getSin()); + moduleDeltaMatrix.set(i * 2 + 1, 0, module.distance * module.angle.getSin()); } var chassisDeltaVector = m_firstOrderForwardKinematics.mult(moduleDeltaMatrix); diff --git a/wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp b/wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp index 02c58edc848..edda2efc64a 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp @@ -30,8 +30,8 @@ namespace wpi::math { * never call it, then this class will behave like regular encoder odometry. */ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator - : public PoseEstimator { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp b/wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp index 075160b147a..4093b4dc419 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp @@ -34,8 +34,8 @@ namespace wpi::math { * never call it, then this class will behave like regular encoder odometry. */ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d - : public PoseEstimator3d { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp b/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp index 60bd0efc4a8..95dd1500848 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp @@ -32,7 +32,7 @@ namespace wpi::math { * odometry. */ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator - : public PoseEstimator { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp b/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp index 544886dced3..849531e6056 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp @@ -36,8 +36,8 @@ namespace wpi::math { * odometry. */ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d - : public PoseEstimator3d { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp index a256344c88a..2ca2c45a1a3 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp @@ -41,7 +41,7 @@ namespace wpi::math { * @tparam WheelPositions Wheel positions type. * @tparam WheelAccelerations Wheel accelerations type. */ -template class WPILIB_DLLEXPORT PoseEstimator { public: @@ -63,8 +63,8 @@ class WPILIB_DLLEXPORT PoseEstimator { * less. */ PoseEstimator( - Kinematics& kinematics, - Odometry& odometry, + Kinematics& kinematics, + Odometry& odometry, const wpi::util::array& stateStdDevs, const wpi::util::array& visionMeasurementStdDevs) : m_odometry(odometry) { @@ -429,7 +429,7 @@ class WPILIB_DLLEXPORT PoseEstimator { static constexpr wpi::units::second_t kBufferDuration = 1.5_s; - Odometry& m_odometry; + Odometry& m_odometry; wpi::util::array m_q{wpi::util::empty_array}; Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); diff --git a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp index 66bd8e3b603..6a26f9db5bc 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp @@ -47,7 +47,7 @@ namespace wpi::math { * @tparam WheelPositions Wheel positions type. * @tparam WheelAccelerations Wheel accelerations type. */ -template class WPILIB_DLLEXPORT PoseEstimator3d { public: @@ -69,8 +69,8 @@ class WPILIB_DLLEXPORT PoseEstimator3d { * vision pose measurement less. */ PoseEstimator3d( - Kinematics& kinematics, - Odometry3d& odometry, + Kinematics& kinematics, + Odometry3d& odometry, const wpi::util::array& stateStdDevs, const wpi::util::array& visionMeasurementStdDevs) : m_odometry(odometry) { @@ -444,7 +444,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { static constexpr wpi::units::second_t kBufferDuration = 1.5_s; - Odometry3d& m_odometry; + Odometry3d& m_odometry; wpi::util::array m_q{wpi::util::empty_array}; wpi::math::Matrixd<6, 6> m_visionK = wpi::math::Matrixd<6, 6>::Zero(); diff --git a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp index 663f2e7ace4..d65cb0b5412 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp @@ -31,8 +31,8 @@ namespace wpi::math { template class SwerveDrivePoseEstimator : public PoseEstimator< - wpi::util::array, wpi::util::array, + wpi::util::array, wpi::util::array> { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp index 102ebc92999..2700270f910 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp @@ -35,8 +35,8 @@ namespace wpi::math { template class SwerveDrivePoseEstimator3d : public PoseEstimator3d< - wpi::util::array, wpi::util::array, + wpi::util::array, wpi::util::array> { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp index 7e9c82b77a8..aab02a10e03 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp @@ -28,8 +28,8 @@ namespace wpi::math { * component velocities into a linear and angular chassis speed. */ class WPILIB_DLLEXPORT DifferentialDriveKinematics - : public Kinematics { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp index a2d888e8da2..013b8a203d7 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp @@ -26,8 +26,8 @@ namespace wpi::math { * Any subsequent pose resets also require the encoders to be reset to zero. */ class WPILIB_DLLEXPORT DifferentialDriveOdometry - : public Odometry { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp index a24be4c9e0d..85a7ade5fcc 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp @@ -26,8 +26,8 @@ namespace wpi::math { * Any subsequent pose resets also require the encoders to be reset to zero. */ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d - : public Odometry3d { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp index b8bae103535..254f0f98fc9 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp @@ -19,7 +19,7 @@ namespace wpi::math { * Inverse kinematics converts a desired chassis speed into wheel speeds whereas * forward kinematics converts wheel speeds into chassis speed. */ -template requires std::copy_constructible && std::assignable_from diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp index 6f59e841bd5..09723ac7c06 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp @@ -43,7 +43,7 @@ namespace wpi::math { * the robot on the field using encoders and a gyro. */ class WPILIB_DLLEXPORT MecanumDriveKinematics - : public Kinematics { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry.hpp index 3984ece54b8..19fb6f45a8f 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry.hpp @@ -25,7 +25,7 @@ namespace wpi::math { * when using computer-vision systems. */ class WPILIB_DLLEXPORT MecanumDriveOdometry - : public Odometry { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry3d.hpp index af7894171f6..d35de415b2f 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry3d.hpp @@ -25,7 +25,7 @@ namespace wpi::math { * when using computer-vision systems. */ class WPILIB_DLLEXPORT MecanumDriveOdometry3d - : public Odometry3d { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp index 320b1181df4..f32f8ff5372 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp @@ -26,7 +26,7 @@ namespace wpi::math { * @tparam WheelPositions Wheel positions type. * @tparam WheelAccelerations Wheel accelerations type. */ -template class WPILIB_DLLEXPORT Odometry { public: @@ -38,7 +38,7 @@ class WPILIB_DLLEXPORT Odometry { * @param wheelPositions The current distances measured by each wheel. * @param initialPose The starting position of the robot on the field. */ - explicit Odometry(const Kinematics& kinematics, const Rotation2d& gyroAngle, const WheelPositions& wheelPositions, @@ -134,7 +134,7 @@ class WPILIB_DLLEXPORT Odometry { } private: - const Kinematics& + const Kinematics& m_kinematics; Pose2d m_pose; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp index 4d0191db65d..7895fbd5cb8 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp @@ -29,7 +29,7 @@ namespace wpi::math { * @tparam WheelPositions Wheel positions type. * @tparam WheelAccelerations Wheel accelerations type. */ -template class WPILIB_DLLEXPORT Odometry3d { public: @@ -41,7 +41,7 @@ class WPILIB_DLLEXPORT Odometry3d { * @param wheelPositions The current distances measured by each wheel. * @param initialPose The starting position of the robot on the field. */ - explicit Odometry3d(const Kinematics& kinematics, const Rotation3d& gyroAngle, const WheelPositions& wheelPositions, @@ -143,7 +143,7 @@ class WPILIB_DLLEXPORT Odometry3d { } private: - const Kinematics& + const Kinematics& m_kinematics; Pose3d m_pose; diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp index c24fdf26a3f..85b767b6d13 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp @@ -53,8 +53,8 @@ namespace wpi::math { template class SwerveDriveKinematics : public Kinematics< - wpi::util::array, wpi::util::array, + wpi::util::array, wpi::util::array> { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp index b6d9d1db637..9011c2893ca 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp @@ -29,8 +29,8 @@ namespace wpi::math { */ template class SwerveDriveOdometry - : public Odometry, - wpi::util::array, + : public Odometry, + wpi::util::array, wpi::util::array> { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp index 84910d7ba81..c709447ee24 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp @@ -31,8 +31,8 @@ namespace wpi::math { template class SwerveDriveOdometry3d : public Odometry3d< - wpi::util::array, wpi::util::array, + wpi::util::array, wpi::util::array> { public: /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp index 5388ef8859b..37e78f1fc61 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp @@ -12,7 +12,7 @@ template <> struct WPILIB_DLLEXPORT wpi::util::Protobuf { - using MessageStruct = wpi_proto_ProtobufSwerveModuleAccelerations; + using MessageStruct = wpi_proto_ProtobufSwerveModuleAcceleration; using InputStream = wpi::util::ProtoInputStream; using OutputStream = diff --git a/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml b/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml index 33f99446963..8e4736fab3a 100644 --- a/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml +++ b/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml @@ -15,8 +15,8 @@ extra_includes: classes: wpi::math::PoseEstimator: template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds - WheelAccelerations methods: PoseEstimator: @@ -38,36 +38,36 @@ templates: DifferentialDrivePoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelAccelerations MecanumDrivePoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive3PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive4PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive6PoseEstimatorBase: qualname: wpi::math::PoseEstimator params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array diff --git a/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml b/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml index 40453de0f47..5d89fca22b8 100644 --- a/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml +++ b/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml @@ -14,8 +14,8 @@ extra_includes: classes: wpi::math::PoseEstimator3d: template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds - WheelAccelerations methods: PoseEstimator3d: @@ -38,36 +38,36 @@ templates: DifferentialDrivePoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelAccelerations MecanumDrivePoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive3PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive4PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive6PoseEstimator3dBase: qualname: wpi::math::PoseEstimator3d params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array diff --git a/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml index 6ce0b74ee19..3654aacb915 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/Kinematics.yml @@ -14,8 +14,8 @@ classes: force_type_casters: - wpi::util::array template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds - WheelAccelerations methods: ToChassisSpeeds: @@ -30,36 +30,36 @@ templates: DifferentialDriveKinematicsBase: qualname: wpi::math::Kinematics params: - - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelAccelerations MecanumDriveKinematicsBase: qualname: wpi::math::Kinematics params: - - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2KinematicsBase: qualname: wpi::math::Kinematics params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive3KinematicsBase: qualname: wpi::math::Kinematics params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive4KinematicsBase: qualname: wpi::math::Kinematics params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive6KinematicsBase: qualname: wpi::math::Kinematics params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array diff --git a/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml b/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml index d3832285357..713f62be627 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml @@ -11,8 +11,8 @@ extra_includes: classes: wpi::math::Odometry: template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds - WheelAccelerations methods: Odometry: @@ -27,36 +27,36 @@ templates: DifferentialDriveOdometryBase: qualname: wpi::math::Odometry params: - - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelAccelerations MecanumDriveOdometryBase: qualname: wpi::math::Odometry params: - - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2OdometryBase: qualname: wpi::math::Odometry params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive3OdometryBase: qualname: wpi::math::Odometry params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive4OdometryBase: qualname: wpi::math::Odometry params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive6OdometryBase: qualname: wpi::math::Odometry params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array diff --git a/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml b/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml index 4bfa19236a6..3bbbb4918b9 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml @@ -11,8 +11,8 @@ extra_includes: classes: wpi::math::Odometry3d: template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds - WheelAccelerations methods: Odometry3d: @@ -28,36 +28,36 @@ templates: DifferentialDriveOdometry3dBase: qualname: wpi::math::Odometry3d params: - - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelPositions + - wpi::math::DifferentialDriveWheelSpeeds - wpi::math::DifferentialDriveWheelAccelerations MecanumDriveOdometry3dBase: qualname: wpi::math::Odometry3d params: - - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelPositions + - wpi::math::MecanumDriveWheelSpeeds - wpi::math::MecanumDriveWheelAccelerations SwerveDrive2Odometry3dBase: qualname: wpi::math::Odometry3d params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive3Odometry3dBase: qualname: wpi::math::Odometry3d params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive4Odometry3dBase: qualname: wpi::math::Odometry3d params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array SwerveDrive6Odometry3dBase: qualname: wpi::math::Odometry3d params: - - wpi::util::array - wpi::util::array + - wpi::util::array - wpi::util::array From 7f81a4ba32d96aa46c1a0434d86dd1126340fdbc Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Wed, 26 Nov 2025 17:14:25 -0500 Subject: [PATCH 80/82] fix unpack method to use mutable angle in SwerveModuleAcceleration Signed-off-by: Zach Harel --- .../math/kinematics/proto/SwerveModuleAccelerationProto.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java index 020d21d035a..d6fef37baf6 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java @@ -30,7 +30,7 @@ public ProtobufSwerveModuleAcceleration createMessage() { @Override public SwerveModuleAcceleration unpack(ProtobufSwerveModuleAcceleration msg) { return new SwerveModuleAcceleration( - msg.getAcceleration(), Rotation2d.proto.unpack(msg.getAngle())); + msg.getAcceleration(), Rotation2d.proto.unpack(msg.getMutableAngle())); } @Override From 56c89d22a8ed29750c44e4b62bfcc5c2b8b7945f Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Wed, 26 Nov 2025 17:19:27 -0500 Subject: [PATCH 81/82] rename SwerveModuleAccelerations to SwerveModuleAcceleration for consistency in robotpy Signed-off-by: Zach Harel --- wpimath/robotpy_pybind_build_info.bzl | 8 ++++---- wpimath/src/main/python/pyproject.toml | 2 +- .../semiwrap/kinematics/SwerveDriveKinematics.yml | 2 +- .../kinematics/SwerveModuleAccelerations.yml | 14 -------------- 4 files changed, 6 insertions(+), 20 deletions(-) delete mode 100644 wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAccelerations.yml diff --git a/wpimath/robotpy_pybind_build_info.bzl b/wpimath/robotpy_pybind_build_info.bzl index 691814a2cae..15cb62fd245 100644 --- a/wpimath/robotpy_pybind_build_info.bzl +++ b/wpimath/robotpy_pybind_build_info.bzl @@ -823,13 +823,13 @@ def wpimath_kinematics_extension(srcs = [], header_to_dat_deps = [], extra_hdrs ], ), struct( - class_name = "SwerveModuleAccelerations", - yml_file = "semiwrap/kinematics/SwerveModuleAccelerations.yml", + class_name = "SwerveModuleAcceleration", + yml_file = "semiwrap/kinematics/SwerveModuleAcceleration.yml", header_root = "$(execpath :robotpy-native-wpimath.copy_headers)", - header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/SwerveModuleAccelerations.hpp", + header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/kinematics/SwerveModuleAcceleration.hpp", tmpl_class_names = [], trampolines = [ - ("wpi::math::SwerveModuleAccelerations", "wpi__math__SwerveModuleAccelerations.hpp"), + ("wpi::math::SwerveModuleAcceleration", "wpi__math__SwerveModuleAcceleration.hpp"), ], ), ] diff --git a/wpimath/src/main/python/pyproject.toml b/wpimath/src/main/python/pyproject.toml index 82351f9c656..d9261ac670c 100644 --- a/wpimath/src/main/python/pyproject.toml +++ b/wpimath/src/main/python/pyproject.toml @@ -1547,7 +1547,7 @@ SwerveDriveOdometry = "wpi/math/kinematics/SwerveDriveOdometry.hpp" SwerveDriveOdometry3d = "wpi/math/kinematics/SwerveDriveOdometry3d.hpp" SwerveModulePosition = "wpi/math/kinematics/SwerveModulePosition.hpp" SwerveModuleState = "wpi/math/kinematics/SwerveModuleState.hpp" -SwerveModuleAccelerations = "wpi/math/kinematics/SwerveModuleAccelerations.hpp" +SwerveModuleAcceleration = "wpi/math/kinematics/SwerveModuleAcceleration.hpp" [tool.semiwrap.extension_modules."wpimath.spline._spline"] diff --git a/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml index d1c36cd5e09..9321c937197 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml @@ -83,7 +83,7 @@ classes: overloads: ModuleAccelerations&&... [const]: ignore: true - const wpi::util::array& [const]: + const wpi::util::array& [const]: template_inline_code: | if constexpr (NumModules == 2) { diff --git a/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAccelerations.yml b/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAccelerations.yml deleted file mode 100644 index 296023b519f..00000000000 --- a/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAccelerations.yml +++ /dev/null @@ -1,14 +0,0 @@ -classes: - wpi::math::SwerveModuleAccelerations: - attributes: - acceleration: - angle: - methods: - operator==: - operator+: - operator-: - overloads: - const SwerveModuleAccelerations& [const]: - '[const]': - operator*: - operator/: From 5a51d44df27dfff8fa7dc55b065da2cdde7f7c8b Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Wed, 26 Nov 2025 17:25:40 -0500 Subject: [PATCH 82/82] add python SwerveModuleAcceleration class definition with attributes and operators Signed-off-by: Zach Harel --- .../kinematics/SwerveModuleAcceleration.yml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAcceleration.yml diff --git a/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAcceleration.yml b/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAcceleration.yml new file mode 100644 index 00000000000..3757525732a --- /dev/null +++ b/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAcceleration.yml @@ -0,0 +1,14 @@ +classes: + wpi::math::SwerveModuleAcceleration: + attributes: + acceleration: + angle: + methods: + operator==: + operator+: + operator-: + overloads: + const SwerveModuleAcceleration& [const]: + '[const]': + operator*: + operator/: