diff --git a/wpimath/robotpy_pybind_build_info.bzl b/wpimath/robotpy_pybind_build_info.bzl index e6e21ae000..15cb62fd24 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 = "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/SwerveModuleAcceleration.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::math::SwerveModuleAcceleration", "wpi__math__SwerveModuleAcceleration.hpp"), + ], + ), ] resolve_casters( 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 acd6b05d3f..0969647a8d 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,80 +19,107 @@ import us.hebi.quickbuf.RepeatedByte; import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3017, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(4185, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + - "BW9tZWdhIkUKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh4KCnRyYWNrd2lkdGgY" + - "ASABKAFSCnRyYWNrd2lkdGgiUAokUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsU3BlZWRzEhIK" + - "BGxlZnQYASABKAFSBGxlZnQSFAoFcmlnaHQYAiABKAFSBXJpZ2h0IlMKJ1Byb3RvYnVmRGlmZmVyZW50" + - "aWFsRHJpdmVXaGVlbFBvc2l0aW9ucxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVy" + - "aWdodCKkAgoeUHJvdG9idWZNZWNhbnVtRHJpdmVLaW5lbWF0aWNzEj8KCmZyb250X2xlZnQYASABKAsy" + - "IC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglmcm9udExlZnQSQQoLZnJvbnRfcmlnaHQY" + - "AiABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgpmcm9udFJpZ2h0Ej0KCXJlYXJf" + - "bGVmdBgDIAEoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSCHJlYXJMZWZ0Ej8KCnJl" + - "YXJfcmlnaHQYBCABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglyZWFyUmlnaHQi" + - "oAEKIlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxQb3NpdGlvbnMSHQoKZnJvbnRfbGVmdBgBIAEoAVIJ" + - "ZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVmdBgDIAEo" + - "AVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0Ip0BCh9Qcm90b2J1Zk1lY2Fu" + - "dW1Ecml2ZVdoZWVsU3BlZWRzEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9udF9y" + - "aWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJlYXJf" + - "cmlnaHQYBCABKAFSCXJlYXJSaWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURyaXZlS2luZW1hdGljcxI6Cgdt" + - "b2R1bGVzGAEgAygLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIHbW9kdWxlcyJvChxQ" + - "cm90b2J1ZlN3ZXJ2ZU1vZHVsZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVh" + - "bmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVm", - "U3dlcnZlTW9kdWxlU3RhdGUSFAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3Bp" + - "LnByb3RvLlByb3RvYnVmUm90YXRpb24yZFIFYW5nbGVCFwoVb3JnLndwaWxpYi5tYXRoLnByb3RvSpkN" + - "CgYSBAAAPQEKCAoBDBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAaCggKAQgSAwYALgoJCgIIARIDBgAu" + - "CgoKAgQAEgQIAAwBCgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoMCgUEAAIABRIDCQIICgwKBQQAAgAB" + - "EgMJCQsKDAoFBAACAAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAACAQUSAwoCCAoMCgUEAAIBARIDCgkL" + - "CgwKBQQAAgEDEgMKDg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgMLAggKDAoFBAACAgESAwsJDgoMCgUE" + - "AAICAxIDCxESCgoKAgQBEgQOABABCgoKAwQBARIDDggrCgsKBAQBAgASAw8CGAoMCgUEAQIABRIDDwII" + - "CgwKBQQBAgABEgMPCRMKDAoFBAECAAMSAw8WFwoKCgIEAhIEEgAVAQoKCgMEAgESAxIILAoLCgQEAgIA" + - "EgMTAhIKDAoFBAICAAUSAxMCCAoMCgUEAgIAARIDEwkNCgwKBQQCAgADEgMTEBEKCwoEBAICARIDFAIT" + - "CgwKBQQCAgEFEgMUAggKDAoFBAICAQESAxQJDgoMCgUEAgIBAxIDFBESCgoKAgQDEgQXABoBCgoKAwQD" + - "ARIDFwgvCgsKBAQDAgASAxgCEgoMCgUEAwIABRIDGAIICgwKBQQDAgABEgMYCQ0KDAoFBAMCAAMSAxgQ" + - "EQoLCgQEAwIBEgMZAhMKDAoFBAMCAQUSAxkCCAoMCgUEAwIBARIDGQkOCgwKBQQDAgEDEgMZERIKCgoC" + - "BAQSBBwAIQEKCgoDBAQBEgMcCCYKCwoEBAQCABIDHQInCgwKBQQEAgAGEgMdAhcKDAoFBAQCAAESAx0Y" + - "IgoMCgUEBAIAAxIDHSUmCgsKBAQEAgESAx4CKAoMCgUEBAIBBhIDHgIXCgwKBQQEAgEBEgMeGCMKDAoF" + - "BAQCAQMSAx4mJwoLCgQEBAICEgMfAiYKDAoFBAQCAgYSAx8CFwoMCgUEBAICARIDHxghCgwKBQQEAgID" + - "EgMfJCUKCwoEBAQCAxIDIAInCgwKBQQEAgMGEgMgAhcKDAoFBAQCAwESAyAYIgoMCgUEBAIDAxIDICUm" + - "CgoKAgQFEgQjACgBCgoKAwQFARIDIwgqCgsKBAQFAgASAyQCGAoMCgUEBQIABRIDJAIICgwKBQQFAgAB" + - "EgMkCRMKDAoFBAUCAAMSAyQWFwoLCgQEBQIBEgMlAhkKDAoFBAUCAQUSAyUCCAoMCgUEBQIBARIDJQkU" + - "CgwKBQQFAgEDEgMlFxgKCwoEBAUCAhIDJgIXCgwKBQQFAgIFEgMmAggKDAoFBAUCAgESAyYJEgoMCgUE" + - "BQICAxIDJhUWCgsKBAQFAgMSAycCGAoMCgUEBQIDBRIDJwIICgwKBQQFAgMBEgMnCRMKDAoFBAUCAwMS", - "AycWFwoKCgIEBhIEKgAvAQoKCgMEBgESAyoIJwoLCgQEBgIAEgMrAhgKDAoFBAYCAAUSAysCCAoMCgUE" + - "BgIAARIDKwkTCgwKBQQGAgADEgMrFhcKCwoEBAYCARIDLAIZCgwKBQQGAgEFEgMsAggKDAoFBAYCAQES" + - "AywJFAoMCgUEBgIBAxIDLBcYCgsKBAQGAgISAy0CFwoMCgUEBgICBRIDLQIICgwKBQQGAgIBEgMtCRIK" + - "DAoFBAYCAgMSAy0VFgoLCgQEBgIDEgMuAhgKDAoFBAYCAwUSAy4CCAoMCgUEBgIDARIDLgkTCgwKBQQG" + - "AgMDEgMuFhcKCgoCBAcSBDEAMwEKCgoDBAcBEgMxCCUKCwoEBAcCABIDMgItCgwKBQQHAgAEEgMyAgoK" + - "DAoFBAcCAAYSAzILIAoMCgUEBwIAARIDMiEoCgwKBQQHAgADEgMyKywKCgoCBAgSBDUAOAEKCgoDBAgB" + - "EgM1CCQKCwoEBAgCABIDNgIWCgwKBQQIAgAFEgM2AggKDAoFBAgCAAESAzYJEQoMCgUECAIAAxIDNhQV" + - "CgsKBAQIAgESAzcCHwoMCgUECAIBBhIDNwIUCgwKBQQIAgEBEgM3FRoKDAoFBAgCAQMSAzcdHgoKCgIE" + - "CRIEOgA9AQoKCgMECQESAzoIIQoLCgQECQIAEgM7AhMKDAoFBAkCAAUSAzsCCAoMCgUECQIAARIDOwkO" + - "CgwKBQQJAgADEgM7ERIKCwoEBAkCARIDPAIfCgwKBQQJAgEGEgM8AhQKDAoFBAkCAQESAzwVGgoMCgUE" + - "CQIBAxIDPB0eYgZwcm90bzM="); + "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" + + "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()); 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_ProtobufDifferentialDriveWheelAccelerations_descriptor = descriptor.internalContainedType(367, 87, "ProtobufDifferentialDriveWheelAccelerations", "wpi.proto.ProtobufDifferentialDriveWheelAccelerations"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(662, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(456, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(825, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(542, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(984, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(837, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1077, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(1000, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1190, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelAccelerations_descriptor = descriptor.internalContainedType(1160, 164, "ProtobufMecanumDriveWheelAccelerations", "wpi.proto.ProtobufMecanumDriveWheelAccelerations"); + + 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_ProtobufSwerveModuleAcceleration_descriptor = descriptor.internalContainedType(1636, 123, "ProtobufSwerveModuleAcceleration", "wpi.proto.ProtobufSwerveModuleAcceleration"); /** * @return this proto file's descriptor. @@ -513,6 +540,420 @@ public final class Kinematics { } } + /** + * 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} */ @@ -1109,6 +1550,344 @@ public final class Kinematics { } } + /** + * Protobuf type {@code ProtobufDifferentialDriveWheelAccelerations} + */ + public static final class ProtobufDifferentialDriveWheelAccelerations extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double left = 1; + */ + private double left; + + /** + * optional double right = 2; + */ + private double right; + + private ProtobufDifferentialDriveWheelAccelerations() { + } + + /** + * @return a new empty instance of {@code ProtobufDifferentialDriveWheelAccelerations} + */ + public static ProtobufDifferentialDriveWheelAccelerations newInstance() { + return new ProtobufDifferentialDriveWheelAccelerations(); + } + + /** + * optional double left = 1; + * @return whether the left field is set + */ + public boolean hasLeft() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double left = 1; + * @return this + */ + public ProtobufDifferentialDriveWheelAccelerations clearLeft() { + bitField0_ &= ~0x00000001; + left = 0D; + return this; + } + + /** + * optional double left = 1; + * @return the left + */ + public double getLeft() { + return left; + } + + /** + * optional double left = 1; + * @param value the left to set + * @return this + */ + public ProtobufDifferentialDriveWheelAccelerations setLeft(final double value) { + bitField0_ |= 0x00000001; + left = value; + return this; + } + + /** + * optional double right = 2; + * @return whether the right field is set + */ + public boolean hasRight() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double right = 2; + * @return this + */ + public ProtobufDifferentialDriveWheelAccelerations clearRight() { + bitField0_ &= ~0x00000002; + right = 0D; + return this; + } + + /** + * optional double right = 2; + * @return the right + */ + public double getRight() { + return right; + } + + /** + * optional double right = 2; + * @param value the right to set + * @return this + */ + public ProtobufDifferentialDriveWheelAccelerations setRight(final double value) { + bitField0_ |= 0x00000002; + right = value; + return this; + } + + @Override + public ProtobufDifferentialDriveWheelAccelerations copyFrom( + final ProtobufDifferentialDriveWheelAccelerations other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + left = other.left; + right = other.right; + } + return this; + } + + @Override + public ProtobufDifferentialDriveWheelAccelerations mergeFrom( + final ProtobufDifferentialDriveWheelAccelerations other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasLeft()) { + setLeft(other.left); + } + if (other.hasRight()) { + setRight(other.right); + } + return this; + } + + @Override + public ProtobufDifferentialDriveWheelAccelerations clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + left = 0D; + right = 0D; + return this; + } + + @Override + public ProtobufDifferentialDriveWheelAccelerations 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 ProtobufDifferentialDriveWheelAccelerations)) { + return false; + } + ProtobufDifferentialDriveWheelAccelerations other = (ProtobufDifferentialDriveWheelAccelerations) 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 ProtobufDifferentialDriveWheelAccelerations 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 ProtobufDifferentialDriveWheelAccelerations 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 ProtobufDifferentialDriveWheelAccelerations clone() { + return new ProtobufDifferentialDriveWheelAccelerations().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufDifferentialDriveWheelAccelerations parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelAccelerations(), data).checkInitialized(); + } + + public static ProtobufDifferentialDriveWheelAccelerations parseFrom(final ProtoSource input) + throws IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelAccelerations(), input).checkInitialized(); + } + + public static ProtobufDifferentialDriveWheelAccelerations parseFrom(final JsonSource input) + throws IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelAccelerations(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufDifferentialDriveWheelAccelerations messages + */ + public static MessageFactory getFactory() { + return ProtobufDifferentialDriveWheelAccelerationsFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Kinematics.wpi_proto_ProtobufDifferentialDriveWheelAccelerations_descriptor; + } + + private enum ProtobufDifferentialDriveWheelAccelerationsFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufDifferentialDriveWheelAccelerations create() { + return ProtobufDifferentialDriveWheelAccelerations.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 ProtobufDifferentialDriveWheelPositions} */ @@ -3032,6 +3811,508 @@ public final class Kinematics { } } + /** + * Protobuf type {@code ProtobufMecanumDriveWheelAccelerations} + */ + public static final class ProtobufMecanumDriveWheelAccelerations 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 ProtobufMecanumDriveWheelAccelerations() { + } + + /** + * @return a new empty instance of {@code ProtobufMecanumDriveWheelAccelerations} + */ + public static ProtobufMecanumDriveWheelAccelerations newInstance() { + return new ProtobufMecanumDriveWheelAccelerations(); + } + + /** + * 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 ProtobufMecanumDriveWheelAccelerations 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 ProtobufMecanumDriveWheelAccelerations 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 ProtobufMecanumDriveWheelAccelerations 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 ProtobufMecanumDriveWheelAccelerations 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 ProtobufMecanumDriveWheelAccelerations 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 ProtobufMecanumDriveWheelAccelerations 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 ProtobufMecanumDriveWheelAccelerations 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 ProtobufMecanumDriveWheelAccelerations setRearRight(final double value) { + bitField0_ |= 0x00000008; + rearRight = value; + return this; + } + + @Override + public ProtobufMecanumDriveWheelAccelerations copyFrom( + final ProtobufMecanumDriveWheelAccelerations other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + frontLeft = other.frontLeft; + frontRight = other.frontRight; + rearLeft = other.rearLeft; + rearRight = other.rearRight; + } + return this; + } + + @Override + public ProtobufMecanumDriveWheelAccelerations mergeFrom( + final ProtobufMecanumDriveWheelAccelerations other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasFrontLeft()) { + setFrontLeft(other.frontLeft); + } + if (other.hasFrontRight()) { + setFrontRight(other.frontRight); + } + if (other.hasRearLeft()) { + setRearLeft(other.rearLeft); + } + if (other.hasRearRight()) { + setRearRight(other.rearRight); + } + return this; + } + + @Override + public ProtobufMecanumDriveWheelAccelerations clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + frontLeft = 0D; + frontRight = 0D; + rearLeft = 0D; + rearRight = 0D; + return this; + } + + @Override + public ProtobufMecanumDriveWheelAccelerations 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 ProtobufMecanumDriveWheelAccelerations)) { + return false; + } + ProtobufMecanumDriveWheelAccelerations other = (ProtobufMecanumDriveWheelAccelerations) o; + return bitField0_ == other.bitField0_ + && (!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) 9); + output.writeDoubleNoTag(frontLeft); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(frontRight); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeRawByte((byte) 25); + output.writeDoubleNoTag(rearLeft); + } + if ((bitField0_ & 0x00000008) != 0) { + output.writeRawByte((byte) 33); + output.writeDoubleNoTag(rearRight); + } + } + + @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; + } + if ((bitField0_ & 0x00000008) != 0) { + size += 9; + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufMecanumDriveWheelAccelerations mergeFrom(final ProtoSource input) throws + IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 9: { + // frontLeft + frontLeft = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 17) { + break; + } + } + case 17: { + // frontRight + frontRight = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 25) { + break; + } + } + case 25: { + // rearLeft + rearLeft = input.readDouble(); + bitField0_ |= 0x00000004; + tag = input.readTag(); + if (tag != 33) { + break; + } + } + case 33: { + // rearRight + rearRight = input.readDouble(); + 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.writeDouble(FieldNames.frontLeft, frontLeft); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.frontRight, frontRight); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeDouble(FieldNames.rearLeft, rearLeft); + } + if ((bitField0_ & 0x00000008) != 0) { + output.writeDouble(FieldNames.rearRight, rearRight); + } + output.endObject(); + } + + @Override + public ProtobufMecanumDriveWheelAccelerations 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()) { + frontLeft = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case -336370317: + case -1456996218: { + if (input.isAtField(FieldNames.frontRight)) { + if (!input.trySkipNullValue()) { + frontRight = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + case -854852661: + case -712874558: { + if (input.isAtField(FieldNames.rearLeft)) { + if (!input.trySkipNullValue()) { + rearLeft = input.readDouble(); + bitField0_ |= 0x00000004; + } + } else { + input.skipUnknownField(); + } + break; + } + case -724967720: + case -618613823: { + if (input.isAtField(FieldNames.rearRight)) { + if (!input.trySkipNullValue()) { + rearRight = input.readDouble(); + bitField0_ |= 0x00000008; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufMecanumDriveWheelAccelerations clone() { + return new ProtobufMecanumDriveWheelAccelerations().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufMecanumDriveWheelAccelerations parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelAccelerations(), data).checkInitialized(); + } + + public static ProtobufMecanumDriveWheelAccelerations parseFrom(final ProtoSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelAccelerations(), input).checkInitialized(); + } + + public static ProtobufMecanumDriveWheelAccelerations parseFrom(final JsonSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufMecanumDriveWheelAccelerations(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufMecanumDriveWheelAccelerations messages + */ + public static MessageFactory getFactory() { + return ProtobufMecanumDriveWheelAccelerationsFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Kinematics.wpi_proto_ProtobufMecanumDriveWheelAccelerations_descriptor; + } + + private enum ProtobufMecanumDriveWheelAccelerationsFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufMecanumDriveWheelAccelerations create() { + return ProtobufMecanumDriveWheelAccelerations.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 ProtobufSwerveDriveKinematics} */ @@ -4027,4 +5308,360 @@ public final class Kinematics { static final FieldName angle = FieldName.forField("angle"); } } + + /** + * Protobuf type {@code ProtobufSwerveModuleAcceleration} + */ + public static final class ProtobufSwerveModuleAcceleration extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double acceleration = 1; + */ + private double acceleration; + + /** + * optional .wpi.proto.ProtobufRotation2d angle = 2; + */ + private final Geometry2D.ProtobufRotation2d angle = Geometry2D.ProtobufRotation2d.newInstance(); + + private ProtobufSwerveModuleAcceleration() { + } + + /** + * @return a new empty instance of {@code ProtobufSwerveModuleAcceleration} + */ + public static ProtobufSwerveModuleAcceleration newInstance() { + return new ProtobufSwerveModuleAcceleration(); + } + + /** + * 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 ProtobufSwerveModuleAcceleration 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 ProtobufSwerveModuleAcceleration setAcceleration(final double value) { + bitField0_ |= 0x00000001; + acceleration = value; + return this; + } + + /** + * optional .wpi.proto.ProtobufRotation2d angle = 2; + * @return whether the angle field is set + */ + public boolean hasAngle() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional .wpi.proto.ProtobufRotation2d angle = 2; + * @return this + */ + public ProtobufSwerveModuleAcceleration clearAngle() { + bitField0_ &= ~0x00000002; + angle.clear(); + return this; + } + + /** + * 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 Geometry2D.ProtobufRotation2d getAngle() { + return angle; + } + + /** + * 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 ProtobufSwerveModuleAcceleration setAngle(final Geometry2D.ProtobufRotation2d value) { + bitField0_ |= 0x00000002; + angle.copyFrom(value); + return this; + } + + @Override + public ProtobufSwerveModuleAcceleration copyFrom(final ProtobufSwerveModuleAcceleration other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + acceleration = other.acceleration; + angle.copyFrom(other.angle); + } + return this; + } + + @Override + public ProtobufSwerveModuleAcceleration mergeFrom( + final ProtobufSwerveModuleAcceleration other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasAcceleration()) { + setAcceleration(other.acceleration); + } + if (other.hasAngle()) { + getMutableAngle().mergeFrom(other.angle); + } + return this; + } + + @Override + public ProtobufSwerveModuleAcceleration clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + acceleration = 0D; + angle.clear(); + return this; + } + + @Override + public ProtobufSwerveModuleAcceleration clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + angle.clearQuick(); + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufSwerveModuleAcceleration)) { + return false; + } + ProtobufSwerveModuleAcceleration other = (ProtobufSwerveModuleAcceleration) o; + return bitField0_ == other.bitField0_ + && (!hasAcceleration() || ProtoUtil.isEqual(acceleration, other.acceleration)) + && (!hasAngle() || angle.equals(other.angle)); + } + + @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) 18); + output.writeMessageNoTag(angle); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += 1 + ProtoSink.computeMessageSizeNoTag(angle); + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufSwerveModuleAcceleration 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 != 18) { + break; + } + } + case 18: { + // angle + input.readMessage(angle); + 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.writeMessage(FieldNames.angle, angle); + } + output.endObject(); + } + + @Override + public ProtobufSwerveModuleAcceleration 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 92960979: { + if (input.isAtField(FieldNames.angle)) { + if (!input.trySkipNullValue()) { + input.readMessage(angle); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufSwerveModuleAcceleration clone() { + return new ProtobufSwerveModuleAcceleration().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufSwerveModuleAcceleration parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAcceleration(), data).checkInitialized(); + } + + public static ProtobufSwerveModuleAcceleration parseFrom(final ProtoSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAcceleration(), input).checkInitialized(); + } + + public static ProtobufSwerveModuleAcceleration parseFrom(final JsonSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufSwerveModuleAcceleration(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufSwerveModuleAcceleration messages + */ + public static MessageFactory getFactory() { + return ProtobufSwerveModuleAccelerationFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Kinematics.wpi_proto_ProtobufSwerveModuleAcceleration_descriptor; + } + + private enum ProtobufSwerveModuleAccelerationFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufSwerveModuleAcceleration create() { + return ProtobufSwerveModuleAcceleration.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName acceleration = FieldName.forField("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 2e095f07ec..e60b09a19e 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,296 +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,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,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,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,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,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, +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"; @@ -322,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}; } @@ -334,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}; } @@ -358,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}; } @@ -376,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_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) + + #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 0f8900c8cb..f73aa9daf1 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_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_ProtobufSwerveModuleAcceleration; + /* 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_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} #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_ProtobufSwerveModuleAcceleration_init_zero {0, {{NULL}, NULL}} /* 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_ProtobufSwerveModuleAcceleration_acceleration_tag 1 +#define wpi_proto_ProtobufSwerveModuleAcceleration_angle_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,16 +313,27 @@ 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_ProtobufSwerveModuleAcceleration_FIELDLIST(X, a) \ +X(a, STATIC, SINGULAR, DOUBLE, acceleration, 1) \ +X(a, CALLBACK, OPTIONAL, MESSAGE, angle, 2) +#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_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 #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 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 f1967f7a81..0e86941138 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 6406d5dc5e..d9ab292d2a 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/ChassisAccelerations.java b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java new file mode 100644 index 0000000000..ad68f54067 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java @@ -0,0 +1,200 @@ +// 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 org.wpilib.math.kinematics; + +import static org.wpilib.units.Units.MetersPerSecondPerSecond; +import static org.wpilib.units.Units.RadiansPerSecondPerSecond; + +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; + +/** + * 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 + * will often have all three components. + */ +public class ChassisAccelerations + implements ProtobufSerializable, StructSerializable, Interpolatable { + /** Acceleration along the x-axis in meters per second squared. (Fwd is +) */ + public double ax; + + /** Acceleration along the y-axis in meters per second squared. (Left is +) */ + public double ay; + + /** Angular acceleration of the robot frame in radians per second squared. (CCW is +) */ + public double alpha; + + /** 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() {} + + /** + * Constructs a ChassisAccelerations object. + * + * @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; + this.ay = ay; + this.alpha = alpha; + } + + /** + * Constructs a ChassisAccelerations object. + * + * @param ax Forward acceleration. + * @param ay Sideways acceleration. + * @param alpha Angular acceleration. + */ + public ChassisAccelerations( + LinearAcceleration ax, LinearAcceleration ay, AngularAcceleration alpha) { + this( + ax.in(MetersPerSecondPerSecond), + ay.in(MetersPerSecondPerSecond), + alpha.in(RadiansPerSecondPerSecond)); + } + + /** + * 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. + */ + 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 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. + */ + 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.lerp(this.ax, endValue.ax, t), + MathUtil.lerp(this.ay, endValue.ay, t), + MathUtil.lerp(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(Ax: %.2f m/s², Ay: %.2f m/s², Alpha: %.2f rad/s²)", ax, ay, alpha); + } +} 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 8eaa72ea99..e84f99ce35 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java @@ -12,8 +12,10 @@ 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.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; @@ -28,7 +30,8 @@ import org.wpilib.util.struct.StructSerializable; * 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 +202,20 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable { 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.lerp(this.vx, endValue.vx, t), + MathUtil.lerp(this.vy, endValue.vy, t), + MathUtil.lerp(this.omega, endValue.omega, t)); + } + } + @Override public final int hashCode() { return Objects.hash(vx, vy, omega); 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 0e8c92907b..9dad84bc18 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveKinematics.java @@ -23,7 +23,10 @@ import org.wpilib.util.struct.StructSerializable; * chassis speed. */ public class DifferentialDriveKinematics - implements Kinematics, + implements Kinematics< + DifferentialDriveWheelPositions, + DifferentialDriveWheelSpeeds, + DifferentialDriveWheelAccelerations>, ProtobufSerializable, StructSerializable { /** Differential drive trackwidth in meters. */ @@ -88,6 +91,23 @@ public class DifferentialDriveKinematics 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/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerations.java b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerations.java new file mode 100644 index 0000000000..2bdbbf95e7 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerations.java @@ -0,0 +1,148 @@ +// 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 org.wpilib.math.kinematics; + +import static org.wpilib.units.Units.MetersPerSecondPerSecond; + +import org.wpilib.math.interpolation.Interpolatable; +import org.wpilib.math.kinematics.proto.DifferentialDriveWheelAccelerationsProto; +import org.wpilib.math.kinematics.struct.DifferentialDriveWheelAccelerationsStruct; +import org.wpilib.units.measure.LinearAcceleration; +import org.wpilib.util.protobuf.ProtobufSerializable; +import org.wpilib.util.struct.StructSerializable; + +/** Represents the wheel accelerations for a differential drive drivetrain. */ +public class DifferentialDriveWheelAccelerations + implements Interpolatable, + 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); + } + + /** + * 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( + "DifferentialDriveWheelAccelerations(Left: %.2f m/s², Right: %.2f m/s²)", left, right); + } +} diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeeds.java index 7ada1bda2c..831a890fa0 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeeds.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeeds.java @@ -6,6 +6,7 @@ package org.wpilib.math.kinematics; import static org.wpilib.units.Units.MetersPerSecond; +import org.wpilib.math.interpolation.Interpolatable; import org.wpilib.math.kinematics.proto.DifferentialDriveWheelSpeedsProto; import org.wpilib.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct; import org.wpilib.units.measure.LinearVelocity; @@ -13,7 +14,10 @@ import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.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 class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc 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/org/wpilib/math/kinematics/Kinematics.java b/wpimath/src/main/java/org/wpilib/math/kinematics/Kinematics.java index 444b4dd94a..624b6126b0 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/Kinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/Kinematics.java @@ -8,14 +8,15 @@ import org.wpilib.math.geometry.Twist2d; import org.wpilib.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 positions. + * @param The type of the wheel speeds. + * @param The type of the wheel accelerations. */ -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/org/wpilib/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveKinematics.java index 72ed9e5822..0d7151aae7 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveKinematics.java @@ -34,7 +34,8 @@ import org.wpilib.util.struct.StructSerializable; * field using encoders and a gyro. */ public class MecanumDriveKinematics - implements Kinematics, + implements Kinematics< + MecanumDriveWheelPositions, MecanumDriveWheelSpeeds, MecanumDriveWheelAccelerations>, ProtobufSerializable, StructSerializable { private final SimpleMatrix m_inverseKinematics; @@ -162,6 +163,88 @@ public class MecanumDriveKinematics 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/org/wpilib/math/kinematics/MecanumDriveWheelAccelerations.java b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelAccelerations.java new file mode 100644 index 0000000000..8d79132eb6 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelAccelerations.java @@ -0,0 +1,184 @@ +// 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 org.wpilib.math.kinematics; + +import static org.wpilib.units.Units.MetersPerSecondPerSecond; + +import org.wpilib.math.interpolation.Interpolatable; +import org.wpilib.math.kinematics.proto.MecanumDriveWheelAccelerationsProto; +import org.wpilib.math.kinematics.struct.MecanumDriveWheelAccelerationsStruct; +import org.wpilib.units.measure.LinearAcceleration; +import org.wpilib.util.protobuf.ProtobufSerializable; +import org.wpilib.util.struct.StructSerializable; + +/** Represents the wheel accelerations for a mecanum drive drivetrain. */ +public class MecanumDriveWheelAccelerations + implements Interpolatable, + 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); + } + + /** + * 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( + "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/org/wpilib/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelSpeeds.java index e95e96d2e3..4aa8d04666 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelSpeeds.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelSpeeds.java @@ -6,6 +6,7 @@ package org.wpilib.math.kinematics; import static org.wpilib.units.Units.MetersPerSecond; +import org.wpilib.math.interpolation.Interpolatable; import org.wpilib.math.kinematics.proto.MecanumDriveWheelSpeedsProto; import org.wpilib.math.kinematics.struct.MecanumDriveWheelSpeedsStruct; import org.wpilib.units.measure.LinearVelocity; @@ -13,7 +14,8 @@ import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.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 class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri 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/org/wpilib/math/kinematics/Odometry.java b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java index d6a6fe5629..79dcd243cf 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 @@ import org.wpilib.math.geometry.Translation2d; * @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 bcdd4a6ab7..dd68b4d7d3 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 @@ import org.wpilib.math.geometry.Twist3d; * @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 82c3cb05aa..35bbf99a36 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java @@ -43,11 +43,13 @@ import org.wpilib.util.struct.StructSerializable; */ @SuppressWarnings("overrides") public class SwerveDriveKinematics - implements Kinematics, + 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,13 @@ public class SwerveDriveKinematics 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_forwardKinematics = m_inverseKinematics.pseudoInverse(); + setInverseKinematics(Translation2d.kZero); + + m_firstOrderForwardKinematics = m_firstOrderInverseKinematics.pseudoInverse(); + m_secondOrderForwardKinematics = m_secondOrderInverseKinematics.pseudoInverse(); MathSharedStore.reportUsage("SwerveDriveKinematics", ""); } @@ -132,19 +134,13 @@ public class SwerveDriveKinematics } 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; + setInverseKinematics(centerOfRotation); } 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 +197,7 @@ public class SwerveDriveKinematics 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), @@ -229,10 +225,10 @@ public class SwerveDriveKinematics 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_forwardKinematics.mult(moduleDeltaMatrix); + var chassisDeltaVector = m_firstOrderForwardKinematics.mult(moduleDeltaMatrix); return new Twist2d( chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0)); } @@ -441,4 +437,155 @@ public class SwerveDriveKinematics 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. + * + * @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 SwerveModuleAcceleration[] 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 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 SwerveModuleAcceleration(0.0, Rotation2d.kZero); + } + return moduleAccelerations; + } + + if (!centerOfRotation.equals(m_prevCoR)) { + setInverseKinematics(centerOfRotation); + } + + var chassisAccelerationsVector = new SimpleMatrix(4, 1); + chassisAccelerationsVector.setColumn( + 0, + 0, + chassisAccelerations.ax, + chassisAccelerations.ay, + angularVelocity * angularVelocity, + chassisAccelerations.alpha); + + var moduleAccelerationsMatrix = m_secondOrderInverseKinematics.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); + + if (linearAcceleration <= 1e-6) { + moduleAccelerations[i] = new SwerveModuleAcceleration(linearAcceleration, Rotation2d.kZero); + } else { + moduleAccelerations[i] = + new SwerveModuleAcceleration(linearAcceleration, new Rotation2d(x, y)); + } + } + + return moduleAccelerations; + } + + /** + * Performs inverse kinematics. See {@link #toSwerveModuleAccelerations(ChassisAccelerations, + * 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 SwerveModuleAcceleration[] toSwerveModuleAccelerations( + ChassisAccelerations chassisAccelerations, double angularVelocity) { + return toSwerveModuleAccelerations(chassisAccelerations, angularVelocity, Translation2d.kZero); + } + + @Override + public SwerveModuleAcceleration[] toWheelAccelerations( + ChassisAccelerations chassisAccelerations) { + return toSwerveModuleAccelerations(chassisAccelerations, 0.0); + } + + /** + * 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( + 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 + + 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]; + + 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_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(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/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java new file mode 100644 index 0000000000..26ff4427e0 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java @@ -0,0 +1,106 @@ +// 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 org.wpilib.math.kinematics; + +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.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 SwerveModuleAcceleration + implements Interpolatable, + Comparable, + ProtobufSerializable, + StructSerializable { + /** Acceleration of the wheel of the module in meters per second squared. */ + public double acceleration; + + /** Angle of the acceleration vector. */ + public Rotation2d angle = new Rotation2d(); + + /** SwerveModuleAccelerations protobuf for serialization. */ + public static final SwerveModuleAccelerationProto proto = new SwerveModuleAccelerationProto(); + + /** SwerveModuleAccelerations struct for serialization. */ + public static final SwerveModuleAccelerationStruct struct = new SwerveModuleAccelerationStruct(); + + /** Constructs a SwerveModuleAccelerations with zeros for acceleration and angle. */ + public SwerveModuleAcceleration() {} + + /** + * Constructs a 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 SwerveModuleAcceleration(double acceleration, Rotation2d angle) { + this.acceleration = acceleration; + this.angle = angle; + } + + /** + * Constructs a SwerveModuleAccelerations. + * + * @param acceleration The acceleration of the wheel of the module. + * @param angle The angle of the acceleration vector. + */ + public SwerveModuleAcceleration(LinearAcceleration acceleration, Rotation2d angle) { + this(acceleration.in(MetersPerSecondPerSecond), angle); + } + + /** + * 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 SwerveModuleAcceleration interpolate(SwerveModuleAcceleration endValue, double t) { + // Clamp t to [0, 1] + t = Math.max(0.0, Math.min(1.0, t)); + + return new SwerveModuleAcceleration( + acceleration + t * (endValue.acceleration - acceleration), + angle.interpolate(endValue.angle, t)); + } + + @Override + public boolean equals(Object obj) { + return obj instanceof SwerveModuleAcceleration other + && Math.abs(other.acceleration - acceleration) < 1E-9 + && angle.equals(other.angle); + } + + @Override + public int hashCode() { + return Objects.hash(acceleration, angle); + } + + /** + * Compares two swerve module accelerations. One swerve module is "greater" than the other if its + * 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. + */ + @Override + public int compareTo(SwerveModuleAcceleration other) { + return Double.compare(this.acceleration, other.acceleration); + } + + @Override + public String toString() { + return String.format( + "SwerveModuleAccelerations(Acceleration: %.2f m/s², Angle: %s)", acceleration, angle); + } +} 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 47282efc20..30335f3d56 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleState.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleState.java @@ -8,6 +8,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; import org.wpilib.math.kinematics.struct.SwerveModuleStateStruct; import org.wpilib.units.measure.LinearVelocity; @@ -16,7 +17,10 @@ import org.wpilib.util.struct.StructSerializable; /** 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 class SwerveModuleState } } + /** + * 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/main/java/org/wpilib/math/kinematics/proto/ChassisAccelerationsProto.java b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/ChassisAccelerationsProto.java new file mode 100644 index 0000000000..0a33c7688a --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/ChassisAccelerationsProto.java @@ -0,0 +1,40 @@ +// 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 org.wpilib.math.kinematics.proto; + +import org.wpilib.math.kinematics.ChassisAccelerations; +import org.wpilib.math.proto.Kinematics.ProtobufChassisAccelerations; +import org.wpilib.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/java/org/wpilib/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java new file mode 100644 index 0000000000..27103c9d42 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.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 org.wpilib.math.kinematics.proto; + +import org.wpilib.math.kinematics.DifferentialDriveWheelAccelerations; +import org.wpilib.math.proto.Kinematics.ProtobufDifferentialDriveWheelAccelerations; +import org.wpilib.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveWheelAccelerationsProto + implements Protobuf< + DifferentialDriveWheelAccelerations, ProtobufDifferentialDriveWheelAccelerations> { + @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/org/wpilib/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java new file mode 100644 index 0000000000..cecbd2cce3 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.math.kinematics.proto; + +import org.wpilib.math.kinematics.MecanumDriveWheelAccelerations; +import org.wpilib.math.proto.Kinematics.ProtobufMecanumDriveWheelAccelerations; +import org.wpilib.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/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java new file mode 100644 index 0000000000..00fa8930da --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.math.kinematics.proto; + +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModuleAcceleration; +import org.wpilib.math.proto.Kinematics.ProtobufSwerveModuleAcceleration; +import org.wpilib.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class SwerveModuleAccelerationProto + implements Protobuf { + @Override + public Class getTypeClass() { + return SwerveModuleAcceleration.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufSwerveModuleAcceleration.getDescriptor(); + } + + @Override + public ProtobufSwerveModuleAcceleration createMessage() { + return ProtobufSwerveModuleAcceleration.newInstance(); + } + + @Override + public SwerveModuleAcceleration unpack(ProtobufSwerveModuleAcceleration msg) { + return new SwerveModuleAcceleration( + msg.getAcceleration(), Rotation2d.proto.unpack(msg.getAngle())); + } + + @Override + public void pack(ProtobufSwerveModuleAcceleration msg, SwerveModuleAcceleration value) { + msg.setAcceleration(value.acceleration); + Rotation2d.proto.pack(msg.getMutableAngle(), value.angle); + } +} 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 new file mode 100644 index 0000000000..0c97139a0a --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/ChassisAccelerationsStruct.java @@ -0,0 +1,46 @@ +// 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 org.wpilib.math.kinematics.struct; + +import java.nio.ByteBuffer; +import org.wpilib.math.kinematics.ChassisAccelerations; +import org.wpilib.util.struct.Struct; + +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); + } +} 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 new file mode 100644 index 0000000000..8441901ac6 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.math.kinematics.struct; + +import java.nio.ByteBuffer; +import org.wpilib.math.kinematics.DifferentialDriveWheelAccelerations; +import org.wpilib.util.struct.Struct; + +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/org/wpilib/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java new file mode 100644 index 0000000000..38f9777d5f --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.math.kinematics.struct; + +import java.nio.ByteBuffer; +import org.wpilib.math.kinematics.MecanumDriveWheelAccelerations; +import org.wpilib.util.struct.Struct; + +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/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationStruct.java b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationStruct.java new file mode 100644 index 0000000000..aeeb4a7524 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationStruct.java @@ -0,0 +1,50 @@ +// 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 org.wpilib.math.kinematics.struct; + +import java.nio.ByteBuffer; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModuleAcceleration; +import org.wpilib.util.struct.Struct; + +public class SwerveModuleAccelerationStruct implements Struct { + @Override + public Class getTypeClass() { + return SwerveModuleAcceleration.class; + } + + @Override + public String getTypeName() { + return "SwerveModuleAccelerations"; + } + + @Override + public int getSize() { + return kSizeDouble + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "double acceleration;Rotation2d angle"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Rotation2d.struct}; + } + + @Override + public SwerveModuleAcceleration unpack(ByteBuffer bb) { + double acceleration = bb.getDouble(); + Rotation2d angle = Rotation2d.struct.unpack(bb); + return new SwerveModuleAcceleration(acceleration, angle); + } + + @Override + 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/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index 2de6ec0a47..1e9d954028 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -87,3 +87,54 @@ 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 wheelAccelerationsVector{ + wheelAccelerations.frontLeft.value(), + wheelAccelerations.frontRight.value(), + wheelAccelerations.rearLeft.value(), + wheelAccelerations.rearRight.value()}; + + 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)}}; +} + +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 chassisAccelerationsVector{ + chassisAccelerations.ax.value(), chassisAccelerations.ay.value(), + chassisAccelerations.alpha.value()}; + + Eigen::Vector4d wheelsVector = + m_inverseKinematics * chassisAccelerationsVector; + + MecanumDriveWheelAccelerations wheelAccelerations; + wheelAccelerations.frontLeft = + wpi::units::meters_per_second_squared_t{wheelsVector(0)}; + wheelAccelerations.frontRight = + wpi::units::meters_per_second_squared_t{wheelsVector(1)}; + wheelAccelerations.rearLeft = + wpi::units::meters_per_second_squared_t{wheelsVector(2)}; + wheelAccelerations.rearRight = + wpi::units::meters_per_second_squared_t{wheelsVector(3)}; + return wheelAccelerations; +} 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 0000000000..b573d60ff5 --- /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 "wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp" + +#include "wpimath/protobuf/kinematics.npb.h" + +std::optional wpi::util::Protobuf< + wpi::math::ChassisAccelerations>::Unpack(InputStream& stream) { + wpi_proto_ProtobufChassisAccelerations msg; + if (!stream.Decode(msg)) { + return {}; + } + + 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::util::Protobuf::Pack( + OutputStream& stream, const wpi::math::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/proto/DifferentialDriveWheelAccelerationsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.cpp new file mode 100644 index 0000000000..b39542fdc3 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelAccelerationsProto.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 "wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp" + +#include "wpimath/protobuf/kinematics.npb.h" + +std::optional +wpi::util::Protobuf::Unpack( + InputStream& stream) { + wpi_proto_ProtobufDifferentialDriveWheelAccelerations msg; + if (!stream.Decode(msg)) { + return {}; + } + + return wpi::math::DifferentialDriveWheelAccelerations{ + units::meters_per_second_squared_t{msg.left}, + units::meters_per_second_squared_t{msg.right}, + }; +} + +bool wpi::util::Protobuf::Pack( + OutputStream& stream, + const wpi::math::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 0000000000..7311ac426f --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelAccelerationsProto.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 "wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp" + +#include "wpimath/protobuf/kinematics.npb.h" + +std::optional wpi::util::Protobuf< + wpi::math::MecanumDriveWheelAccelerations>::Unpack(InputStream& stream) { + wpi_proto_ProtobufMecanumDriveWheelAccelerations msg; + if (!stream.Decode(msg)) { + return {}; + } + + 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}, + units::meters_per_second_squared_t{msg.rear_right}, + }; +} + +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(), + .rear_left = value.rearLeft.value(), + .rear_right = value.rearRight.value(), + }; + return stream.Encode(msg); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp new file mode 100644 index 0000000000..b8954fe018 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleAccelerationProto.cpp @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#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::SwerveModuleAcceleration>::Unpack(InputStream& stream) { + wpi::util::UnpackCallback angle; + wpi_proto_ProtobufSwerveModuleAcceleration msg{ + .acceleration = 0, + .angle = angle.Callback(), + }; + if (!stream.Decode(msg)) { + return {}; + } + + auto iangle = angle.Items(); + + if (iangle.empty()) { + return {}; + } + + return wpi::math::SwerveModuleAcceleration{ + units::meters_per_second_squared_t{msg.acceleration}, + iangle[0], + }; +} + +bool wpi::util::Protobuf::Pack( + OutputStream& stream, const wpi::math::SwerveModuleAcceleration& value) { + wpi::util::PackCallback angle{&value.angle}; + wpi_proto_ProtobufSwerveModuleAcceleration msg{ + .acceleration = value.acceleration.value(), + .angle = angle.Callback(), + }; + 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 0000000000..1a5113b0c6 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisAccelerationsStruct.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 "wpi/math/kinematics/struct/ChassisAccelerationsStruct.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) { + constexpr size_t kAxOff = 0; + constexpr size_t kAyOff = kAxOff + 8; + constexpr size_t kAlphaOff = kAyOff + 8; + return wpi::math::ChassisAccelerations{ + units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + units::radians_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + }; +} + +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::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 new file mode 100644 index 0000000000..9091888302 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.cpp @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp" + +#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" +#include "wpi/util/struct/Struct.hpp" + +wpi::math::DifferentialDriveWheelAccelerations +wpi::util::Struct::Unpack( + std::span data) { + constexpr size_t kLeftOff = 0; + constexpr size_t kRightOff = kLeftOff + 8; + return wpi::math::DifferentialDriveWheelAccelerations{ + units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + }; +} + +void wpi::util::Struct::Pack( + std::span data, + const wpi::math::DifferentialDriveWheelAccelerations& value) { + constexpr size_t kLeftOff = 0; + constexpr size_t kRightOff = kLeftOff + 8; + 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 new file mode 100644 index 0000000000..2fcd821b1a --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelAccelerationsStruct.cpp @@ -0,0 +1,40 @@ +// 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 "wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp" + +#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" +#include "wpi/util/struct/Struct.hpp" + +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 wpi::math::MecanumDriveWheelAccelerations{ + units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + }; +} + +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::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/SwerveModuleAccelerationStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationStruct.cpp new file mode 100644 index 0000000000..e9a82bd055 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleAccelerationStruct.cpp @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp" + +#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp" +#include "wpi/util/struct/Struct.hpp" + +wpi::math::SwerveModuleAcceleration +wpi::util::Struct::Unpack( + std::span data) { + constexpr size_t kAccelerationOff = 0; + constexpr size_t kAngleOff = kAccelerationOff + 8; + return wpi::math::SwerveModuleAcceleration{ + units::meters_per_second_squared_t{ + wpi::util::UnpackStruct(data)}, + wpi::util::UnpackStruct(data)}; +} + +void wpi::util::Struct::Pack( + 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()); + wpi::util::PackStruct(data, value.angle); +} 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 4cb460f44c..edda2efc64 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,9 @@ namespace wpi::math { * never call it, then this class will behave like regular encoder odometry. */ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator - : public PoseEstimator { + : public PoseEstimator { public: /** * Constructs a DifferentialDrivePoseEstimator with default standard 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 5c626cd240..4093b4dc41 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,9 @@ namespace wpi::math { * never call it, then this class will behave like regular encoder odometry. */ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d - : public PoseEstimator3d { + : public PoseEstimator3d { public: /** * Constructs a DifferentialDrivePoseEstimator3d with default standard 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 41c2003540..95dd150084 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp @@ -32,8 +32,8 @@ namespace wpi::math { * 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/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp b/wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp index 9dc0bdc9b3..849531e605 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,9 @@ namespace wpi::math { * odometry. */ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d - : public PoseEstimator3d { + : public PoseEstimator3d { public: /** * Constructs a MecanumDrivePoseEstimator3d with default standard deviations 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 3e7e98367a..23db711753 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp @@ -37,10 +37,12 @@ namespace wpi::math { * AddVisionMeasurement() can be called as infrequently as you want; if you * never call it, then this class will behave like regular encoder odometry. * - * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. + * @tparam WheelSpeeds Wheel speeds type. + * @tparam WheelAccelerations Wheel accelerations type. */ -template +template class WPILIB_DLLEXPORT PoseEstimator { public: /** @@ -60,10 +62,11 @@ class WPILIB_DLLEXPORT PoseEstimator { * radians). Increase these numbers to trust the vision pose measurement * less. */ - PoseEstimator(Kinematics& kinematics, - Odometry& odometry, - const wpi::util::array& stateStdDevs, - const wpi::util::array& visionMeasurementStdDevs) + PoseEstimator( + Kinematics& kinematics, + Odometry& odometry, + const wpi::util::array& stateStdDevs, + const wpi::util::array& visionMeasurementStdDevs) : m_odometry(odometry) { for (size_t i = 0; i < 3; ++i) { m_q[i] = stateStdDevs[i] * stateStdDevs[i]; @@ -426,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 29bd3f3be7..66dd453341 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp @@ -43,10 +43,12 @@ namespace wpi::math { * AddVisionMeasurement() can be called as infrequently as you want; if you * never call it, then this class will behave like regular encoder odometry. * - * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. + * @tparam WheelSpeeds Wheel speeds type. + * @tparam WheelAccelerations Wheel accelerations type. */ -template +template class WPILIB_DLLEXPORT PoseEstimator3d { public: /** @@ -66,10 +68,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::util::array& stateStdDevs, - const wpi::util::array& visionMeasurementStdDevs) + PoseEstimator3d( + Kinematics& kinematics, + Odometry3d& odometry, + const wpi::util::array& stateStdDevs, + const wpi::util::array& visionMeasurementStdDevs) : m_odometry(odometry) { for (size_t i = 0; i < 4; ++i) { m_q[i] = stateStdDevs[i] * stateStdDevs[i]; @@ -441,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 249fe39248..d65cb0b541 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp @@ -30,8 +30,10 @@ namespace wpi::math { */ template class SwerveDrivePoseEstimator - : public PoseEstimator, - 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/estimator/SwerveDrivePoseEstimator3d.hpp b/wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp index b39e1580a1..2700270f91 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,9 @@ namespace wpi::math { template 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/ChassisAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp new file mode 100644 index 0000000000..f97bf0f295 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp @@ -0,0 +1,166 @@ +// 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/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 { +/** + * 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. + * + * 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. + */ +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; + + /** + * 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, 2.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 wpi::math + +#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 9a296cdc76..aab02a10e0 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" @@ -26,8 +28,9 @@ namespace wpi::math { * component velocities into a linear and angular chassis speed. */ class WPILIB_DLLEXPORT DifferentialDriveKinematics - : public Kinematics { + : public Kinematics { public: /** * Constructs a differential drive kinematics object. @@ -98,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. wpi::units::meter_t trackwidth; }; 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 dc2766b371..013b8a203d 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,9 @@ namespace wpi::math { * Any subsequent pose resets also require the encoders to be reset to zero. */ class WPILIB_DLLEXPORT DifferentialDriveOdometry - : public Odometry { + : public Odometry { public: /** * Constructs a DifferentialDriveOdometry object. 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 d85e27a7ce..85a7ade5fc 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,9 @@ namespace wpi::math { * Any subsequent pose resets also require the encoders to be reset to zero. */ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d - : public Odometry3d { + : public Odometry3d { public: /** * Constructs a DifferentialDriveOdometry3d object. diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp new file mode 100644 index 0000000000..932184a698 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp @@ -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 "wpi/units/acceleration.hpp" +#include "wpi/units/math.hpp" +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::math { +/** + * 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 wpi::math + +#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 091fcbd909..254f0f98fc 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" @@ -18,7 +19,8 @@ namespace wpi::math { * 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 { @@ -74,5 +76,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 wpi::math 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 d33ef1b11a..09723ac7c0 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" @@ -41,7 +43,8 @@ namespace wpi::math { * 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. @@ -171,6 +174,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; 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 8d12779e9b..19fb6f45a8 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,8 @@ namespace wpi::math { * 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/wpi/math/kinematics/MecanumDriveOdometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry3d.hpp index fb2f9c6401..d35de415b2 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,8 @@ namespace wpi::math { * 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/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp new file mode 100644 index 0000000000..713e311995 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp @@ -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 "wpi/units/acceleration.hpp" +#include "wpi/units/math.hpp" +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::math { +/** + * 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 wpi::math + +#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/Odometry.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp index f053587eba..478bed1d90 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp @@ -22,10 +22,12 @@ namespace wpi::math { * path following. Furthermore, odometry can be used for latency compensation * when using computer-vision systems. * - * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. + * @tparam WheelSpeeds Wheel speeds type. + * @tparam WheelAccelerations Wheel accelerations type. */ -template +template class WPILIB_DLLEXPORT Odometry { public: /** @@ -36,7 +38,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{}) @@ -131,7 +134,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/wpi/math/kinematics/Odometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp index 8450632b4c..49e416073e 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp @@ -25,10 +25,12 @@ namespace wpi::math { * path following. Furthermore, odometry can be used for latency compensation * when using computer-vision systems. * - * @tparam WheelSpeeds Wheel speeds type. * @tparam WheelPositions Wheel positions type. + * @tparam WheelSpeeds Wheel speeds type. + * @tparam WheelAccelerations Wheel accelerations type. */ -template +template class WPILIB_DLLEXPORT Odometry3d { public: /** @@ -39,7 +41,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{}) @@ -140,7 +143,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/wpi/math/kinematics/SwerveDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp index 0d347b2509..85b767b6d1 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/SwerveModuleAcceleration.hpp" #include "wpi/math/kinematics/SwerveModulePosition.hpp" #include "wpi/math/kinematics/SwerveModuleState.hpp" #include "wpi/math/linalg/EigenCore.hpp" @@ -50,8 +52,10 @@ namespace wpi::math { */ template class SwerveDriveKinematics - : public Kinematics, - 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 @@ -71,13 +75,20 @@ class SwerveDriveKinematics m_moduleHeadings(wpi::util::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", ""); } @@ -87,13 +98,20 @@ class SwerveDriveKinematics : m_modules{modules}, m_moduleHeadings(wpi::util::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", ""); } @@ -171,15 +189,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(), @@ -187,7 +197,7 @@ class SwerveDriveKinematics chassisSpeeds.omega.value()}; Matrixd moduleStateMatrix = - m_inverseKinematics * chassisSpeedsVector; + m_firstOrderInverseKinematics * chassisSpeedsVector; for (size_t i = 0; i < NumModules; i++) { wpi::units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)}; @@ -254,7 +264,7 @@ class SwerveDriveKinematics } Eigen::Vector3d chassisSpeedsVector = - m_forwardKinematics.solve(moduleStateMatrix); + m_firstOrderForwardKinematics.solve(moduleStateMatrix); return {wpi::units::meters_per_second_t{chassisSpeedsVector(0)}, wpi::units::meters_per_second_t{chassisSpeedsVector(1)}, @@ -307,7 +317,7 @@ class SwerveDriveKinematics } Eigen::Vector3d chassisDeltaVector = - m_forwardKinematics.solve(moduleDeltaMatrix); + m_firstOrderForwardKinematics.solve(moduleDeltaMatrix); return {wpi::units::meter_t{chassisDeltaVector(0)}, wpi::units::meter_t{chassisDeltaVector(1)}, @@ -445,13 +455,188 @@ 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::util::array + ToSwerveModuleAccelerations( + 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::util::array moduleAccelerations( + wpi::util::empty_array); + + if (chassisAccelerations.ax == 0.0_mps_sq && + 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_rad}}; + } + 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 (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(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); + + if (linearAcceleration.value() < 1e-6) { + moduleAccelerations[i] = {linearAcceleration, {}}; + } else { + 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. + * @return An array containing the module accelerations. + */ + wpi::util::array ToWheelAccelerations( + const ChassisAccelerations& chassisAccelerations) const override { + 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. + */ + template < + std::convertible_to... ModuleAccelerations> + requires(sizeof...(ModuleAccelerations) == NumModules) + ChassisAccelerations ToChassisAccelerations( + ModuleAccelerations&&... moduleAccelerations) const { + return this->ToChassisAccelerations( + wpi::util::array{ + moduleAccelerations...}); + } + + /** + * 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::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 + // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf + + Matrixd moduleAccelerationsMatrix; + + for (size_t i = 0; i < NumModules; i++) { + SwerveModuleAcceleration module = moduleAccelerations[i]; + + moduleAccelerationsMatrix(i * 2 + 0, 0) = + module.acceleration.value() * module.angle.Cos(); + moduleAccelerationsMatrix(i * 2 + 1, 0) = + module.acceleration.value() * module.angle.Sin(); + } + + Eigen::Vector4d chassisAccelerationsVector = + m_secondOrderForwardKinematics.solve(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::util::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::util::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) 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; + } }; template 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 dff4b672de..9011c2893c 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,9 @@ namespace wpi::math { */ template class SwerveDriveOdometry - : public Odometry, - wpi::util::array> { + : public Odometry, + 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 80b4ee078e..c709447ee2 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp @@ -30,8 +30,10 @@ namespace wpi::math { */ template class SwerveDriveOdometry3d - : public Odometry3d, - 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/SwerveModuleAcceleration.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAcceleration.hpp new file mode 100644 index 0000000000..0f72d030ed --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAcceleration.hpp @@ -0,0 +1,112 @@ +// 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/geometry/Rotation2d.hpp" +#include "wpi/units/acceleration.hpp" +#include "wpi/units/math.hpp" +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::math { +/** + * Represents the accelerations of one swerve module. + */ +struct WPILIB_DLLEXPORT SwerveModuleAcceleration { + /** + * Acceleration of the wheel of the module. + */ + units::meters_per_second_squared_t acceleration = 0_mps_sq; + + /** + * Angle of the acceleration vector. + */ + Rotation2d angle; + + /** + * Checks equality between this SwerveModuleAccelerations and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + constexpr bool operator==(const SwerveModuleAcceleration& other) const { + return units::math::abs(acceleration - other.acceleration) < 1E-9_mps_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. + */ + SwerveModuleAcceleration operator+( + const SwerveModuleAcceleration& 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}; + } + + /** + * 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. + */ + SwerveModuleAcceleration operator-( + const SwerveModuleAcceleration& other) const { + return *this + (-other); + } + + /** + * Returns the inverse of the current SwerveModuleAccelerations. + * This is equivalent to rotating the acceleration by pi (or 180 degrees). + * + * @return The inverse of the current SwerveModuleAccelerations. + */ + constexpr SwerveModuleAcceleration operator-() const { + return {acceleration, angle + Rotation2d{180_deg}}; + } + + /** + * Multiplies the SwerveModuleAccelerations by a scalar and returns the new + * SwerveModuleAccelerations. + * + * @param scalar The scalar to multiply by. + * @return The scaled SwerveModuleAccelerations. + */ + constexpr SwerveModuleAcceleration operator*(double scalar) const { + if (scalar < 0) { + return {-scalar * acceleration, angle + Rotation2d{180_deg}}; + } + return {scalar * acceleration, angle}; + } + + /** + * Divides the SwerveModuleAccelerations by a scalar and returns the new + * SwerveModuleAccelerations. + * + * @param scalar The scalar to divide by. + * @return The scaled SwerveModuleAccelerations. + */ + constexpr SwerveModuleAcceleration operator/(double scalar) const { + return operator*(1.0 / scalar); + } +}; +} // namespace wpi::math + +#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/ChassisAccelerationsProto.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp new file mode 100644 index 0000000000..7ab7485db2 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.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/ChassisAccelerations.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_ProtobufChassisAccelerations; + 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 new file mode 100644 index 0000000000..8fb7aca097 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.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/DifferentialDriveWheelAccelerations.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_ProtobufDifferentialDriveWheelAccelerations; + 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, + 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 new file mode 100644 index 0000000000..2adaff0498 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.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/MecanumDriveWheelAccelerations.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_ProtobufMecanumDriveWheelAccelerations; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::MecanumDriveWheelAccelerations& value); +}; 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 new file mode 100644 index 0000000000..37e78f1fc6 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationProto.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/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 { + using MessageStruct = wpi_proto_ProtobufSwerveModuleAcceleration; + using InputStream = + wpi::util::ProtoInputStream; + using OutputStream = + wpi::util::ProtoOutputStream; + static std::optional Unpack( + InputStream& stream); + static bool Pack(OutputStream& stream, + const wpi::math::SwerveModuleAcceleration& 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 new file mode 100644 index 0000000000..1279c2e017 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/math/kinematics/ChassisAccelerations.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" + +template <> +struct WPILIB_DLLEXPORT wpi::util::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 wpi::math::ChassisAccelerations Unpack(std::span data); + static void Pack(std::span data, + const wpi::math::ChassisAccelerations& value); +}; + +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 new file mode 100644 index 0000000000..3e32f90813 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" + +template <> +struct WPILIB_DLLEXPORT + wpi::util::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 wpi::math::DifferentialDriveWheelAccelerations Unpack( + std::span data); + static void Pack(std::span data, + const wpi::math::DifferentialDriveWheelAccelerations& value); +}; + +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 new file mode 100644 index 0000000000..6265989687 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" + +template <> +struct WPILIB_DLLEXPORT + wpi::util::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 wpi::math::MecanumDriveWheelAccelerations Unpack( + std::span data); + static void Pack(std::span data, + const wpi::math::MecanumDriveWheelAccelerations& value); +}; + +static_assert( + wpi::util::StructSerializable); 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 new file mode 100644 index 0000000000..8062e8710b --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/struct/Struct.hpp" + +template <> +struct WPILIB_DLLEXPORT wpi::util::Struct { + static constexpr std::string_view GetTypeName() { + return "SwerveModuleAccelerations"; + } + static constexpr size_t GetSize() { + return 8 + wpi::util::GetStructSize(); + } + static constexpr std::string_view GetSchema() { + return "double acceleration;Rotation2d angle"; + } + + static wpi::math::SwerveModuleAcceleration Unpack( + std::span data); + static void Pack(std::span data, + const wpi::math::SwerveModuleAcceleration& value); + static void ForEachNested( + std::invocable auto fn) { + wpi::util::ForEachStructSchema(fn); + } +}; + +static_assert( + 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 4623f199ec..5e5e20527c 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; } @@ -21,6 +27,11 @@ message ProtobufDifferentialDriveWheelSpeeds { double right = 2; } +message ProtobufDifferentialDriveWheelAccelerations { + double left = 1; + double right = 2; +} + message ProtobufDifferentialDriveWheelPositions { double left = 1; double right = 2; @@ -47,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; } @@ -60,3 +78,8 @@ message ProtobufSwerveModuleState { double speed = 1; ProtobufRotation2d angle = 2; } + +message ProtobufSwerveModuleAcceleration { + double acceleration = 1; + ProtobufRotation2d angle = 2; +} diff --git a/wpimath/src/main/python/pyproject.toml b/wpimath/src/main/python/pyproject.toml index ec9c249dbb..34f1916670 100644 --- a/wpimath/src/main/python/pyproject.toml +++ b/wpimath/src/main/python/pyproject.toml @@ -1529,17 +1529,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" @@ -1547,6 +1550,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" +SwerveModuleAcceleration = "wpi/math/kinematics/SwerveModuleAcceleration.hpp" [tool.semiwrap.extension_modules."wpimath.spline._spline"] diff --git a/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml b/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml index 5e95d1dde3..afa7f35b24 100644 --- a/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml +++ b/wpimath/src/main/python/semiwrap/controls/PoseEstimator.yml @@ -2,18 +2,22 @@ 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/SwerveModuleAcceleration.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp classes: wpi::math::PoseEstimator: template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds + - WheelAccelerations methods: PoseEstimator: SetVisionMeasurementStdDevs: @@ -34,30 +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 69cd62b6e6..72d4bd0f25 100644 --- a/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml +++ b/wpimath/src/main/python/semiwrap/controls/PoseEstimator3d.yml @@ -2,17 +2,21 @@ 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/SwerveModuleAcceleration.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp classes: wpi::math::PoseEstimator3d: template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds + - WheelAccelerations methods: PoseEstimator3d: SetVisionMeasurementStdDevs: @@ -34,30 +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/ChassisAccelerations.yml b/wpimath/src/main/python/semiwrap/kinematics/ChassisAccelerations.yml new file mode 100644 index 0000000000..d6eb353f70 --- /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 04b2ee336b..76168dac20 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 0000000000..47c41a4f0b --- /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 57b87c12e2..7849598556 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/SwerveModuleAcceleration.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp @@ -11,43 +14,52 @@ classes: force_type_casters: - wpi::util::array template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds + - WheelAccelerations methods: ToChassisSpeeds: ToWheelSpeeds: ToTwist2d: Interpolate: + ToChassisAccelerations: + ToWheelAccelerations: 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/MecanumDriveKinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveKinematics.yml index b97bf4ee4e..219e7d4928 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 0000000000..f69a33d8b1 --- /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/Odometry.yml b/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml index d144ac62ad..bd8fde1dc0 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/Odometry.yml @@ -1,15 +1,19 @@ 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/SwerveModuleAcceleration.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp classes: wpi::math::Odometry: template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds + - WheelAccelerations methods: Odometry: ResetPosition: @@ -23,30 +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 6b1657f5f4..27e5d43b41 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/Odometry3d.yml @@ -1,15 +1,19 @@ 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/SwerveModuleAcceleration.hpp - wpi/math/kinematics/SwerveDriveKinematics.hpp classes: wpi::math::Odometry3d: template_params: - - WheelSpeeds - WheelPositions + - WheelSpeeds + - WheelAccelerations methods: Odometry3d: ResetPosition: @@ -24,30 +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 diff --git a/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml b/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml index 141fd96668..9321c93719 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/SwerveDriveKinematics.yml @@ -77,6 +77,13 @@ classes: Interpolate: GetModules: + ToSwerveModuleAccelerations: + ToWheelAccelerations: + ToChassisAccelerations: + overloads: + ModuleAccelerations&&... [const]: + ignore: true + const wpi::util::array& [const]: template_inline_code: | if constexpr (NumModules == 2) { 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 0000000000..3757525732 --- /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/: diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/ChassisAccelerationsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/ChassisAccelerationsTest.java new file mode 100644 index 0000000000..863ca72c82 --- /dev/null +++ b/wpimath/src/test/java/org/wpilib/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 org.wpilib.math.kinematics; + +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.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation2d; + +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/org/wpilib/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/ChassisSpeedsTest.java index f5424da79d..f2fd868911 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/ChassisSpeedsTest.java @@ -127,4 +127,36 @@ class ChassisSpeedsTest { () -> 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/org/wpilib/math/kinematics/DifferentialDriveKinematicsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveKinematicsTest.java index a4472fa7dc..e83c963848 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveKinematicsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveKinematicsTest.java @@ -76,4 +76,68 @@ class DifferentialDriveKinematicsTest { () -> 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/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerationsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerationsTest.java new file mode 100644 index 0000000000..38022f2ebf --- /dev/null +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerationsTest.java @@ -0,0 +1,137 @@ +// 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 org.wpilib.math.kinematics; + +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; + +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)); + } + + @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/org/wpilib/math/kinematics/DifferentialDriveWheelSpeedsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeedsTest.java index 9df3f3ba98..bc02a39ad6 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeedsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeedsTest.java @@ -55,4 +55,44 @@ class DifferentialDriveWheelSpeedsTest { 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/org/wpilib/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveKinematicsTest.java index 6fd7b18a75..f462c28522 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveKinematicsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveKinematicsTest.java @@ -169,6 +169,112 @@ class MecanumDriveKinematicsTest { () -> 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/org/wpilib/math/kinematics/MecanumDriveWheelAccelerationsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelAccelerationsTest.java new file mode 100644 index 0000000000..328e310a4f --- /dev/null +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelAccelerationsTest.java @@ -0,0 +1,170 @@ +// 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 org.wpilib.math.kinematics; + +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; + +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)); + } + + @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/org/wpilib/math/kinematics/MecanumDriveWheelSpeedsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelSpeedsTest.java index 8570308da9..e5944de950 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelSpeedsTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelSpeedsTest.java @@ -70,4 +70,58 @@ class MecanumDriveWheelSpeedsTest { () -> 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/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java index 3f2ba19da0..984d9e5179 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 @@ package org.wpilib.math.kinematics; 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; @@ -146,12 +147,10 @@ class SwerveDriveKinematicsTest { 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), @@ -199,14 +198,12 @@ class SwerveDriveKinematicsTest { 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,14 +225,12 @@ class SwerveDriveKinematicsTest { 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), @@ -252,14 +247,12 @@ class SwerveDriveKinematicsTest { 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), @@ -312,14 +305,12 @@ class SwerveDriveKinematicsTest { 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), @@ -336,14 +327,12 @@ class SwerveDriveKinematicsTest { 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), @@ -406,4 +395,218 @@ class SwerveDriveKinematicsTest { () -> 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); + double angularVelocity = 2 * Math.PI; + 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. + + 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(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(), + kEpsilon), + () -> + assertEquals( + expectedAngles[1].getDegrees(), + moduleAccelerations[1].angle.getDegrees(), + kEpsilon), + () -> + assertEquals( + expectedAngles[2].getDegrees(), + moduleAccelerations[2].angle.getDegrees(), + kEpsilon), + () -> + assertEquals( + expectedAngles[3].getDegrees(), + moduleAccelerations[3].angle.getDegrees(), + kEpsilon)); + } + + @Test + void testTurnInPlaceForwardAccelerations() { + 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); + + 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); + // 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 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()) + .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, 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(), + kEpsilon), + () -> + assertEquals( + expectedAngles[1].getDegrees(), + moduleAccelerations[1].angle.getDegrees(), + kEpsilon), + () -> + assertEquals( + expectedAngles[2].getDegrees(), + moduleAccelerations[2].angle.getDegrees(), + kEpsilon), + () -> + assertEquals( + expectedAngles[3].getDegrees(), + moduleAccelerations[3].angle.getDegrees(), + kEpsilon)); + } } diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationTest.java new file mode 100644 index 0000000000..8362efd0ad --- /dev/null +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleAccelerationTest.java @@ -0,0 +1,109 @@ +// 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 org.wpilib.math.kinematics; + +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.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Rotation2d; + +class SwerveModuleAccelerationTest { + private static final double kEpsilon = 1E-9; + + @Test + void testDefaultConstructor() { + var moduleAccelerations = new SwerveModuleAcceleration(); + + assertAll( + () -> assertEquals(0.0, moduleAccelerations.acceleration, kEpsilon), + () -> assertEquals(0.0, moduleAccelerations.angle.getRadians(), kEpsilon)); + } + + @Test + void testParameterizedConstructor() { + var moduleAccelerations = new SwerveModuleAcceleration(2.5, new Rotation2d(1.5)); + + assertAll( + () -> assertEquals(2.5, moduleAccelerations.acceleration, kEpsilon), + () -> assertEquals(1.5, moduleAccelerations.angle.getRadians(), kEpsilon)); + } + + @Test + void testMeasureConstructor() { + var acceleration = MetersPerSecondPerSecond.of(3.0); + var angle = new Rotation2d(2.0); + var moduleAccelerations = new SwerveModuleAcceleration(acceleration, angle); + + assertAll( + () -> assertEquals(3.0, moduleAccelerations.acceleration, kEpsilon), + () -> assertEquals(2.0, moduleAccelerations.angle.getRadians(), kEpsilon)); + } + + @Test + void testEquals() { + 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); + } + + @Test + void testCompareTo() { + 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 SwerveModuleAcceleration(1.0, new Rotation2d(2.0)))); + } + + @Test + void testInterpolate() { + 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); + assertAll( + () -> assertEquals(1.0, atStart.acceleration, 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(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(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(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(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(Math.PI / 2, aboveRange.angle.getRadians(), kEpsilon)); + } +} diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleStateTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleStateTest.java index 6d79a95086..0f502f65c3 100644 --- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleStateTest.java +++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleStateTest.java @@ -149,4 +149,55 @@ class SwerveModuleStateTest { () -> 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)); + } } 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 0000000000..909e9a2065 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/ChassisAccelerationsTest.cpp @@ -0,0 +1,101 @@ +// 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 "wpi/math/kinematics/ChassisAccelerations.hpp" + +#include + +#include + +#include "wpi/units/acceleration.hpp" +#include "wpi/units/angular_acceleration.hpp" + +using namespace wpi::math; + +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); +} diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp index cb6cd51db4..28ea4444c5 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp @@ -8,6 +8,7 @@ #include +#include "wpi/math/kinematics/ChassisAccelerations.hpp" #include "wpi/math/kinematics/ChassisSpeeds.hpp" #include "wpi/units/angular_velocity.hpp" #include "wpi/units/length.hpp" @@ -76,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, + wpi::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{ + 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); + + 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/DifferentialDriveWheelAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp new file mode 100644 index 0000000000..9393df18a5 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelAccelerationsTest.cpp @@ -0,0 +1,72 @@ +// 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 "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp" + +#include + +#include "wpi/units/acceleration.hpp" + +using namespace wpi::math; + +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); +} diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index 0b7f60fe49..9fcc551fb7 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -228,3 +228,102 @@ 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, + 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); + 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/MecanumDriveWheelAccelerationsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp new file mode 100644 index 0000000000..591a2df6fe --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelAccelerationsTest.cpp @@ -0,0 +1,94 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp" + +#include + +#include "wpi/units/acceleration.hpp" + +using namespace wpi::math; + +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); +} diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index f6a8983cf6..549eae5957 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -310,3 +310,183 @@ 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, + 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); + + // 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(), 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(), kEpsilon); + EXPECT_NEAR(expectedAngles[1].Degrees().value(), + frAccel.angle.Degrees().value(), kEpsilon); + EXPECT_NEAR(expectedAngles[2].Degrees().value(), + blAccel.angle.Degrees().value(), kEpsilon); + EXPECT_NEAR(expectedAngles[3].Degrees().value(), + brAccel.angle.Degrees().value(), kEpsilon); +} + +TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardAccelerations) { + 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); + + 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) { + ChassisAccelerations accelerations{0_mps_sq, 0_mps_sq, 1_rad_per_s_sq}; + // For this test, assume an angular velocity of 1 rad/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); + + // 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(), 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(), kEpsilon); + EXPECT_NEAR(expectedAngles[1].Degrees().value(), + frAccel.angle.Degrees().value(), kEpsilon); + EXPECT_NEAR(expectedAngles[2].Degrees().value(), + blAccel.angle.Degrees().value(), kEpsilon); + EXPECT_NEAR(expectedAngles[3].Degrees().value(), + brAccel.angle.Degrees().value(), kEpsilon); +} 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 0000000000..d324ddece2 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleAccelerationsTest.cpp @@ -0,0 +1,40 @@ +// 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 "wpi/math/kinematics/SwerveModuleAcceleration.hpp" +#include "wpi/units/acceleration.hpp" + +using namespace wpi::math; + +static constexpr double kEpsilon = 1E-9; + +TEST(SwerveModuleAccelerationsTest, DefaultConstructor) { + SwerveModuleAcceleration moduleAccelerations; + + EXPECT_NEAR(moduleAccelerations.acceleration.value(), 0.0, kEpsilon); + EXPECT_NEAR(moduleAccelerations.angle.Radians().value(), 0.0, kEpsilon); +} + +TEST(SwerveModuleAccelerationsTest, ParameterizedConstructor) { + 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); +} + +TEST(SwerveModuleAccelerationsTest, Equals) { + SwerveModuleAcceleration moduleAccelerations1{2.0_mps_sq, + Rotation2d{1.5_rad}}; + SwerveModuleAcceleration moduleAccelerations2{2.0_mps_sq, + Rotation2d{1.5_rad}}; + SwerveModuleAcceleration moduleAccelerations3{2.1_mps_sq, + Rotation2d{1.5_rad}}; + + EXPECT_EQ(moduleAccelerations1, moduleAccelerations2); + EXPECT_NE(moduleAccelerations1, moduleAccelerations3); +}