mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-06 03:31:43 +00:00
[wpimath] Add ChassisAccelerations and drivetrain accelerations classes and add forward and inverse kinematics for accelerations to the interface (#8185)
ChassisAccelerations and the drivetrain acceleration types are added in both Java and C++. `ChassisAccelerations` is basically just `ChassisSpeeds` but for accelerations! `DifferentialDriveWheelAccelerations`, `MecanumDriveWheelAccelerations`, and `SwerveModuleAccelerations` are the acceleration equivalent of the drivetrain speeds types. In Java, the `Kinematics` interface now has an additional generic parameter `A` which represents the accelerations, and `toChassisAccelerations` and `toWheelAccelerations` methods, which are implemented the same way as `toChassisSpeeds` and `toWheelSpeeds`. Protobuf and struct classes were also added for all four classes in Java and C++. --------- Signed-off-by: Zach Harel <zach@zharel.me> Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Zach Harel
40
wpimath/robotpy_pybind_build_info.bzl
generated
40
wpimath/robotpy_pybind_build_info.bzl
generated
@@ -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(
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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.
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -66,7 +66,7 @@ public class PoseEstimator<T> {
|
||||
*/
|
||||
@SuppressWarnings("PMD.UnusedFormalParameter")
|
||||
public PoseEstimator(
|
||||
Kinematics<?, T> kinematics,
|
||||
Kinematics<T, ?, ?> kinematics,
|
||||
Odometry<T> odometry,
|
||||
Matrix<N3, N1> stateStdDevs,
|
||||
Matrix<N3, N1> visionMeasurementStdDevs) {
|
||||
|
||||
@@ -74,7 +74,7 @@ public class PoseEstimator3d<T> {
|
||||
*/
|
||||
@SuppressWarnings("PMD.UnusedFormalParameter")
|
||||
public PoseEstimator3d(
|
||||
Kinematics<?, T> kinematics,
|
||||
Kinematics<T, ?, ?> kinematics,
|
||||
Odometry3d<T> odometry,
|
||||
Matrix<N4, N1> stateStdDevs,
|
||||
Matrix<N4, N1> visionMeasurementStdDevs) {
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
* <p>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<ChassisAccelerations> {
|
||||
/** 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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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);
|
||||
}
|
||||
}
|
||||
@@ -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<ChassisSpeeds> {
|
||||
/** 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);
|
||||
|
||||
@@ -23,7 +23,10 @@ import org.wpilib.util.struct.StructSerializable;
|
||||
* chassis speed.
|
||||
*/
|
||||
public class DifferentialDriveKinematics
|
||||
implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>,
|
||||
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) {
|
||||
|
||||
@@ -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<DifferentialDriveWheelAccelerations>,
|
||||
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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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);
|
||||
}
|
||||
}
|
||||
@@ -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<DifferentialDriveWheelSpeeds>,
|
||||
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(
|
||||
|
||||
@@ -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 <S> The type of the wheel speeds.
|
||||
* @param <P> The type of the wheel positions.
|
||||
* @param <S> The type of the wheel speeds.
|
||||
* @param <A> The type of the wheel accelerations.
|
||||
*/
|
||||
public interface Kinematics<S, P> extends Interpolator<P> {
|
||||
public interface Kinematics<P, S, A> extends Interpolator<P> {
|
||||
/**
|
||||
* 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<S, P> extends Interpolator<P> {
|
||||
*/
|
||||
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
|
||||
|
||||
@@ -34,7 +34,8 @@ import org.wpilib.util.struct.StructSerializable;
|
||||
* field using encoders and a gyro.
|
||||
*/
|
||||
public class MecanumDriveKinematics
|
||||
implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>,
|
||||
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.
|
||||
*
|
||||
* <p>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);
|
||||
|
||||
@@ -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<MecanumDriveWheelAccelerations>,
|
||||
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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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);
|
||||
}
|
||||
}
|
||||
@@ -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<MecanumDriveWheelSpeeds>, 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(
|
||||
|
||||
@@ -20,7 +20,7 @@ import org.wpilib.math.geometry.Translation2d;
|
||||
* @param <T> Wheel positions type.
|
||||
*/
|
||||
public class Odometry<T> {
|
||||
private final Kinematics<?, T> m_kinematics;
|
||||
private final Kinematics<T, ?, ?> m_kinematics;
|
||||
private Pose2d m_pose;
|
||||
|
||||
private Rotation2d m_gyroOffset;
|
||||
@@ -36,7 +36,7 @@ public class Odometry<T> {
|
||||
* @param initialPose The starting position of the robot on the field.
|
||||
*/
|
||||
public Odometry(
|
||||
Kinematics<?, T> kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) {
|
||||
Kinematics<T, ?, ?> kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) {
|
||||
m_kinematics = kinematics;
|
||||
m_pose = initialPose;
|
||||
m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
|
||||
|
||||
@@ -29,7 +29,7 @@ import org.wpilib.math.geometry.Twist3d;
|
||||
* @param <T> Wheel positions type.
|
||||
*/
|
||||
public class Odometry3d<T> {
|
||||
private final Kinematics<?, T> m_kinematics;
|
||||
private final Kinematics<T, ?, ?> m_kinematics;
|
||||
private Pose3d m_pose;
|
||||
|
||||
private Rotation3d m_gyroOffset;
|
||||
@@ -45,7 +45,7 @@ public class Odometry3d<T> {
|
||||
* @param initialPose The starting position of the robot on the field.
|
||||
*/
|
||||
public Odometry3d(
|
||||
Kinematics<?, T> kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) {
|
||||
Kinematics<T, ?, ?> kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) {
|
||||
m_kinematics = kinematics;
|
||||
m_pose = initialPose;
|
||||
m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
|
||||
|
||||
@@ -43,11 +43,13 @@ import org.wpilib.util.struct.StructSerializable;
|
||||
*/
|
||||
@SuppressWarnings("overrides")
|
||||
public class SwerveDriveKinematics
|
||||
implements Kinematics<SwerveModuleState[], SwerveModulePosition[]>,
|
||||
implements Kinematics<SwerveModulePosition[], SwerveModuleState[], SwerveModuleAcceleration[]>,
|
||||
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.
|
||||
*
|
||||
* <p>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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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<SwerveModuleAcceleration>,
|
||||
Comparable<SwerveModuleAcceleration>,
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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<SwerveModuleState>, ProtobufSerializable, StructSerializable {
|
||||
implements Interpolatable<SwerveModuleState>,
|
||||
Comparable<SwerveModuleState>,
|
||||
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
|
||||
|
||||
@@ -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<ChassisAccelerations, ProtobufChassisAccelerations> {
|
||||
@Override
|
||||
public Class<ChassisAccelerations> 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);
|
||||
}
|
||||
}
|
||||
@@ -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<DifferentialDriveWheelAccelerations> 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);
|
||||
}
|
||||
}
|
||||
@@ -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<MecanumDriveWheelAccelerations, ProtobufMecanumDriveWheelAccelerations> {
|
||||
@Override
|
||||
public Class<MecanumDriveWheelAccelerations> 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);
|
||||
}
|
||||
}
|
||||
@@ -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<SwerveModuleAcceleration, ProtobufSwerveModuleAcceleration> {
|
||||
@Override
|
||||
public Class<SwerveModuleAcceleration> 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);
|
||||
}
|
||||
}
|
||||
@@ -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<ChassisAccelerations> {
|
||||
@Override
|
||||
public Class<ChassisAccelerations> 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);
|
||||
}
|
||||
}
|
||||
@@ -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<DifferentialDriveWheelAccelerations> {
|
||||
@Override
|
||||
public Class<DifferentialDriveWheelAccelerations> 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);
|
||||
}
|
||||
}
|
||||
@@ -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<MecanumDriveWheelAccelerations> {
|
||||
@Override
|
||||
public Class<MecanumDriveWheelAccelerations> 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);
|
||||
}
|
||||
}
|
||||
@@ -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<SwerveModuleAcceleration> {
|
||||
@Override
|
||||
public Class<SwerveModuleAcceleration> 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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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::math::ChassisAccelerations> 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<wpi::math::ChassisAccelerations>::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);
|
||||
}
|
||||
@@ -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::math::DifferentialDriveWheelAccelerations>
|
||||
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelAccelerations>::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<wpi::math::DifferentialDriveWheelAccelerations>::Pack(
|
||||
OutputStream& stream,
|
||||
const wpi::math::DifferentialDriveWheelAccelerations& value) {
|
||||
wpi_proto_ProtobufDifferentialDriveWheelAccelerations msg{
|
||||
.left = value.left.value(),
|
||||
.right = value.right.value(),
|
||||
};
|
||||
return stream.Encode(msg);
|
||||
}
|
||||
@@ -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::math::MecanumDriveWheelAccelerations> 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<wpi::math::MecanumDriveWheelAccelerations>::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);
|
||||
}
|
||||
@@ -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::math::SwerveModuleAcceleration> wpi::util::Protobuf<
|
||||
wpi::math::SwerveModuleAcceleration>::Unpack(InputStream& stream) {
|
||||
wpi::util::UnpackCallback<wpi::math::Rotation2d> 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<wpi::math::SwerveModuleAcceleration>::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);
|
||||
}
|
||||
@@ -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<const uint8_t> 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<double, kAxOff>(data)},
|
||||
units::meters_per_second_squared_t{
|
||||
wpi::util::UnpackStruct<double, kAyOff>(data)},
|
||||
units::radians_per_second_squared_t{
|
||||
wpi::util::UnpackStruct<double, kAlphaOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void wpi::util::Struct<wpi::math::ChassisAccelerations>::Pack(
|
||||
std::span<uint8_t> 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<kAxOff>(data, value.ax.value());
|
||||
wpi::util::PackStruct<kAyOff>(data, value.ay.value());
|
||||
wpi::util::PackStruct<kAlphaOff>(data, value.alpha.value());
|
||||
}
|
||||
@@ -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<wpi::math::DifferentialDriveWheelAccelerations>::Unpack(
|
||||
std::span<const uint8_t> 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<double, kLeftOff>(data)},
|
||||
units::meters_per_second_squared_t{
|
||||
wpi::util::UnpackStruct<double, kRightOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void wpi::util::Struct<wpi::math::DifferentialDriveWheelAccelerations>::Pack(
|
||||
std::span<uint8_t> data,
|
||||
const wpi::math::DifferentialDriveWheelAccelerations& value) {
|
||||
constexpr size_t kLeftOff = 0;
|
||||
constexpr size_t kRightOff = kLeftOff + 8;
|
||||
wpi::util::PackStruct<kLeftOff>(data, value.left.value());
|
||||
wpi::util::PackStruct<kRightOff>(data, value.right.value());
|
||||
}
|
||||
@@ -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<wpi::math::MecanumDriveWheelAccelerations>::Unpack(
|
||||
std::span<const uint8_t> 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<double, kFrontLeftOff>(data)},
|
||||
units::meters_per_second_squared_t{
|
||||
wpi::util::UnpackStruct<double, kFrontRightOff>(data)},
|
||||
units::meters_per_second_squared_t{
|
||||
wpi::util::UnpackStruct<double, kRearLeftOff>(data)},
|
||||
units::meters_per_second_squared_t{
|
||||
wpi::util::UnpackStruct<double, kRearRightOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void wpi::util::Struct<wpi::math::MecanumDriveWheelAccelerations>::Pack(
|
||||
std::span<uint8_t> 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<kFrontLeftOff>(data, value.frontLeft.value());
|
||||
wpi::util::PackStruct<kFrontRightOff>(data, value.frontRight.value());
|
||||
wpi::util::PackStruct<kRearLeftOff>(data, value.rearLeft.value());
|
||||
wpi::util::PackStruct<kRearRightOff>(data, value.rearRight.value());
|
||||
}
|
||||
@@ -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<wpi::math::SwerveModuleAcceleration>::Unpack(
|
||||
std::span<const uint8_t> 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<double, kAccelerationOff>(data)},
|
||||
wpi::util::UnpackStruct<wpi::math::Rotation2d, kAngleOff>(data)};
|
||||
}
|
||||
|
||||
void wpi::util::Struct<wpi::math::SwerveModuleAcceleration>::Pack(
|
||||
std::span<uint8_t> data, const wpi::math::SwerveModuleAcceleration& value) {
|
||||
constexpr size_t kAccelerationOff = 0;
|
||||
constexpr size_t kAngleOff = kAccelerationOff + 8;
|
||||
wpi::util::PackStruct<kAccelerationOff>(data, value.acceleration.value());
|
||||
wpi::util::PackStruct<kAngleOff>(data, value.angle);
|
||||
}
|
||||
@@ -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<DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelPositions> {
|
||||
: public PoseEstimator<DifferentialDriveWheelPositions,
|
||||
DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelAccelerations> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a DifferentialDrivePoseEstimator with default standard
|
||||
|
||||
@@ -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<DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelPositions> {
|
||||
: public PoseEstimator3d<DifferentialDriveWheelPositions,
|
||||
DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelAccelerations> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a DifferentialDrivePoseEstimator3d with default standard
|
||||
|
||||
@@ -32,8 +32,8 @@ namespace wpi::math {
|
||||
* odometry.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
|
||||
: public PoseEstimator<MecanumDriveWheelSpeeds,
|
||||
MecanumDriveWheelPositions> {
|
||||
: public PoseEstimator<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
|
||||
MecanumDriveWheelAccelerations> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a MecanumDrivePoseEstimator with default standard deviations
|
||||
|
||||
@@ -36,8 +36,9 @@ namespace wpi::math {
|
||||
* odometry.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d
|
||||
: public PoseEstimator3d<MecanumDriveWheelSpeeds,
|
||||
MecanumDriveWheelPositions> {
|
||||
: public PoseEstimator3d<MecanumDriveWheelPositions,
|
||||
MecanumDriveWheelSpeeds,
|
||||
MecanumDriveWheelAccelerations> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a MecanumDrivePoseEstimator3d with default standard deviations
|
||||
|
||||
@@ -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 <typename WheelSpeeds, typename WheelPositions>
|
||||
template <typename WheelPositions, typename WheelSpeeds,
|
||||
typename WheelAccelerations>
|
||||
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<WheelSpeeds, WheelPositions>& kinematics,
|
||||
Odometry<WheelSpeeds, WheelPositions>& odometry,
|
||||
const wpi::util::array<double, 3>& stateStdDevs,
|
||||
const wpi::util::array<double, 3>& visionMeasurementStdDevs)
|
||||
PoseEstimator(
|
||||
Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>& kinematics,
|
||||
Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& odometry,
|
||||
const wpi::util::array<double, 3>& stateStdDevs,
|
||||
const wpi::util::array<double, 3>& 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<WheelSpeeds, WheelPositions>& m_odometry;
|
||||
Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
|
||||
wpi::util::array<double, 3> m_q{wpi::util::empty_array};
|
||||
Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
|
||||
|
||||
|
||||
@@ -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 <typename WheelSpeeds, typename WheelPositions>
|
||||
template <typename WheelPositions, typename WheelSpeeds,
|
||||
typename WheelAccelerations>
|
||||
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<WheelSpeeds, WheelPositions>& kinematics,
|
||||
Odometry3d<WheelSpeeds, WheelPositions>& odometry,
|
||||
const wpi::util::array<double, 4>& stateStdDevs,
|
||||
const wpi::util::array<double, 4>& visionMeasurementStdDevs)
|
||||
PoseEstimator3d(
|
||||
Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>& kinematics,
|
||||
Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& odometry,
|
||||
const wpi::util::array<double, 4>& stateStdDevs,
|
||||
const wpi::util::array<double, 4>& 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<WheelSpeeds, WheelPositions>& m_odometry;
|
||||
Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
|
||||
wpi::util::array<double, 4> m_q{wpi::util::empty_array};
|
||||
wpi::math::Matrixd<6, 6> m_visionK = wpi::math::Matrixd<6, 6>::Zero();
|
||||
|
||||
|
||||
@@ -30,8 +30,10 @@ namespace wpi::math {
|
||||
*/
|
||||
template <size_t NumModules>
|
||||
class SwerveDrivePoseEstimator
|
||||
: public PoseEstimator<wpi::util::array<SwerveModuleState, NumModules>,
|
||||
wpi::util::array<SwerveModulePosition, NumModules>> {
|
||||
: public PoseEstimator<
|
||||
wpi::util::array<SwerveModulePosition, NumModules>,
|
||||
wpi::util::array<SwerveModuleState, NumModules>,
|
||||
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a SwerveDrivePoseEstimator with default standard deviations
|
||||
|
||||
@@ -35,8 +35,9 @@ namespace wpi::math {
|
||||
template <size_t NumModules>
|
||||
class SwerveDrivePoseEstimator3d
|
||||
: public PoseEstimator3d<
|
||||
wpi::util::array<SwerveModulePosition, NumModules>,
|
||||
wpi::util::array<SwerveModuleState, NumModules>,
|
||||
wpi::util::array<SwerveModulePosition, NumModules>> {
|
||||
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a SwerveDrivePoseEstimator3d with default standard deviations
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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"
|
||||
@@ -7,7 +7,9 @@
|
||||
#include <type_traits>
|
||||
|
||||
#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<DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelPositions> {
|
||||
: public Kinematics<DifferentialDriveWheelPositions,
|
||||
DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelAccelerations> {
|
||||
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;
|
||||
};
|
||||
|
||||
@@ -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<DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelPositions> {
|
||||
: public Odometry<DifferentialDriveWheelPositions,
|
||||
DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelAccelerations> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a DifferentialDriveOdometry object.
|
||||
|
||||
@@ -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<DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelPositions> {
|
||||
: public Odometry3d<DifferentialDriveWheelPositions,
|
||||
DifferentialDriveWheelSpeeds,
|
||||
DifferentialDriveWheelAccelerations> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a DifferentialDriveOdometry3d object.
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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"
|
||||
@@ -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 <typename WheelSpeeds, typename WheelPositions>
|
||||
template <typename WheelPositions, typename WheelSpeeds,
|
||||
typename WheelAccelerations>
|
||||
requires std::copy_constructible<WheelPositions> &&
|
||||
std::assignable_from<WheelPositions&, const WheelPositions&>
|
||||
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
|
||||
|
||||
@@ -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<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
|
||||
: public Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
|
||||
MecanumDriveWheelAccelerations> {
|
||||
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<Matrixd<4, 3>> m_forwardKinematics;
|
||||
|
||||
@@ -25,7 +25,8 @@ namespace wpi::math {
|
||||
* when using computer-vision systems.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT MecanumDriveOdometry
|
||||
: public Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
|
||||
: public Odometry<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
|
||||
MecanumDriveWheelAccelerations> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a MecanumDriveOdometry object.
|
||||
|
||||
@@ -25,7 +25,8 @@ namespace wpi::math {
|
||||
* when using computer-vision systems.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT MecanumDriveOdometry3d
|
||||
: public Odometry3d<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
|
||||
: public Odometry3d<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
|
||||
MecanumDriveWheelAccelerations> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a MecanumDriveOdometry3d object.
|
||||
|
||||
@@ -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 <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
|
||||
#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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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.
|
||||
*
|
||||
* <p>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"
|
||||
@@ -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 <typename WheelSpeeds, typename WheelPositions>
|
||||
template <typename WheelPositions, typename WheelSpeeds,
|
||||
typename WheelAccelerations>
|
||||
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<WheelSpeeds, WheelPositions>& kinematics,
|
||||
explicit Odometry(const Kinematics<WheelPositions, WheelSpeeds,
|
||||
WheelAccelerations>& kinematics,
|
||||
const Rotation2d& gyroAngle,
|
||||
const WheelPositions& wheelPositions,
|
||||
const Pose2d& initialPose = Pose2d{})
|
||||
@@ -131,7 +134,8 @@ class WPILIB_DLLEXPORT Odometry {
|
||||
}
|
||||
|
||||
private:
|
||||
const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
|
||||
const Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>&
|
||||
m_kinematics;
|
||||
Pose2d m_pose;
|
||||
|
||||
WheelPositions m_previousWheelPositions;
|
||||
|
||||
@@ -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 <typename WheelSpeeds, typename WheelPositions>
|
||||
template <typename WheelPositions, typename WheelSpeeds,
|
||||
typename WheelAccelerations>
|
||||
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<WheelSpeeds, WheelPositions>& kinematics,
|
||||
explicit Odometry3d(const Kinematics<WheelPositions, WheelSpeeds,
|
||||
WheelAccelerations>& kinematics,
|
||||
const Rotation3d& gyroAngle,
|
||||
const WheelPositions& wheelPositions,
|
||||
const Pose3d& initialPose = Pose3d{})
|
||||
@@ -140,7 +143,8 @@ class WPILIB_DLLEXPORT Odometry3d {
|
||||
}
|
||||
|
||||
private:
|
||||
const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
|
||||
const Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>&
|
||||
m_kinematics;
|
||||
Pose3d m_pose;
|
||||
|
||||
WheelPositions m_previousWheelPositions;
|
||||
|
||||
@@ -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 <size_t NumModules>
|
||||
class SwerveDriveKinematics
|
||||
: public Kinematics<wpi::util::array<SwerveModuleState, NumModules>,
|
||||
wpi::util::array<SwerveModulePosition, NumModules>> {
|
||||
: public Kinematics<
|
||||
wpi::util::array<SwerveModulePosition, NumModules>,
|
||||
wpi::util::array<SwerveModuleState, NumModules>,
|
||||
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
|
||||
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<NumModules * 2, 1> 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.
|
||||
*
|
||||
* <p>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<SwerveModuleAcceleration, NumModules>
|
||||
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<SwerveModuleAcceleration, NumModules> 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<NumModules * 2, 1> 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<SwerveModuleAcceleration, NumModules> 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<SwerveModuleAcceleration>... ModuleAccelerations>
|
||||
requires(sizeof...(ModuleAccelerations) == NumModules)
|
||||
ChassisAccelerations ToChassisAccelerations(
|
||||
ModuleAccelerations&&... moduleAccelerations) const {
|
||||
return this->ToChassisAccelerations(
|
||||
wpi::util::array<SwerveModuleAcceleration, NumModules>{
|
||||
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<SwerveModuleAcceleration, NumModules>&
|
||||
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<NumModules * 2, 1> 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<Translation2d, NumModules> m_modules;
|
||||
mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
|
||||
Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
|
||||
mutable Matrixd<NumModules * 2, 3> m_firstOrderInverseKinematics;
|
||||
Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>>
|
||||
m_firstOrderForwardKinematics;
|
||||
mutable Matrixd<NumModules * 2, 4> m_secondOrderInverseKinematics;
|
||||
Eigen::HouseholderQR<Matrixd<NumModules * 2, 4>>
|
||||
m_secondOrderForwardKinematics;
|
||||
mutable wpi::util::array<Rotation2d, NumModules> 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 <typename ModuleTranslation, typename... ModuleTranslations>
|
||||
|
||||
@@ -29,8 +29,9 @@ namespace wpi::math {
|
||||
*/
|
||||
template <size_t NumModules>
|
||||
class SwerveDriveOdometry
|
||||
: public Odometry<wpi::util::array<SwerveModuleState, NumModules>,
|
||||
wpi::util::array<SwerveModulePosition, NumModules>> {
|
||||
: public Odometry<wpi::util::array<SwerveModulePosition, NumModules>,
|
||||
wpi::util::array<SwerveModuleState, NumModules>,
|
||||
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a SwerveDriveOdometry object.
|
||||
|
||||
@@ -30,8 +30,10 @@ namespace wpi::math {
|
||||
*/
|
||||
template <size_t NumModules>
|
||||
class SwerveDriveOdometry3d
|
||||
: public Odometry3d<wpi::util::array<SwerveModuleState, NumModules>,
|
||||
wpi::util::array<SwerveModulePosition, NumModules>> {
|
||||
: public Odometry3d<
|
||||
wpi::util::array<SwerveModulePosition, NumModules>,
|
||||
wpi::util::array<SwerveModuleState, NumModules>,
|
||||
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
|
||||
public:
|
||||
/**
|
||||
* Constructs a SwerveDriveOdometry3d object.
|
||||
|
||||
@@ -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"
|
||||
@@ -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<wpi::math::ChassisAccelerations> {
|
||||
using MessageStruct = wpi_proto_ProtobufChassisAccelerations;
|
||||
using InputStream =
|
||||
wpi::util::ProtoInputStream<wpi::math::ChassisAccelerations>;
|
||||
using OutputStream =
|
||||
wpi::util::ProtoOutputStream<wpi::math::ChassisAccelerations>;
|
||||
static std::optional<wpi::math::ChassisAccelerations> Unpack(
|
||||
InputStream& stream);
|
||||
static bool Pack(OutputStream& stream,
|
||||
const wpi::math::ChassisAccelerations& value);
|
||||
};
|
||||
@@ -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<wpi::math::DifferentialDriveWheelAccelerations> {
|
||||
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelAccelerations;
|
||||
using InputStream = wpi::util::ProtoInputStream<
|
||||
wpi::math::DifferentialDriveWheelAccelerations>;
|
||||
using OutputStream = wpi::util::ProtoOutputStream<
|
||||
wpi::math::DifferentialDriveWheelAccelerations>;
|
||||
static std::optional<wpi::math::DifferentialDriveWheelAccelerations> Unpack(
|
||||
InputStream& stream);
|
||||
static bool Pack(OutputStream& stream,
|
||||
const wpi::math::DifferentialDriveWheelAccelerations& value);
|
||||
};
|
||||
@@ -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<wpi::math::MecanumDriveWheelAccelerations> {
|
||||
using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelAccelerations;
|
||||
using InputStream =
|
||||
wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelAccelerations>;
|
||||
using OutputStream =
|
||||
wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelAccelerations>;
|
||||
static std::optional<wpi::math::MecanumDriveWheelAccelerations> Unpack(
|
||||
InputStream& stream);
|
||||
static bool Pack(OutputStream& stream,
|
||||
const wpi::math::MecanumDriveWheelAccelerations& value);
|
||||
};
|
||||
@@ -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<wpi::math::SwerveModuleAcceleration> {
|
||||
using MessageStruct = wpi_proto_ProtobufSwerveModuleAcceleration;
|
||||
using InputStream =
|
||||
wpi::util::ProtoInputStream<wpi::math::SwerveModuleAcceleration>;
|
||||
using OutputStream =
|
||||
wpi::util::ProtoOutputStream<wpi::math::SwerveModuleAcceleration>;
|
||||
static std::optional<wpi::math::SwerveModuleAcceleration> Unpack(
|
||||
InputStream& stream);
|
||||
static bool Pack(OutputStream& stream,
|
||||
const wpi::math::SwerveModuleAcceleration& value);
|
||||
};
|
||||
@@ -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<wpi::math::ChassisAccelerations> {
|
||||
static constexpr std::string_view GetTypeName() {
|
||||
return "ChassisAccelerations";
|
||||
}
|
||||
static constexpr size_t GetSize() { return 24; }
|
||||
static constexpr std::string_view GetSchema() {
|
||||
return "double ax;double ay;double alpha";
|
||||
}
|
||||
|
||||
static wpi::math::ChassisAccelerations Unpack(std::span<const uint8_t> data);
|
||||
static void Pack(std::span<uint8_t> data,
|
||||
const wpi::math::ChassisAccelerations& value);
|
||||
};
|
||||
|
||||
static_assert(wpi::util::StructSerializable<wpi::math::ChassisAccelerations>);
|
||||
@@ -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<wpi::math::DifferentialDriveWheelAccelerations> {
|
||||
static constexpr std::string_view GetTypeName() {
|
||||
return "DifferentialDriveWheelAccelerations";
|
||||
}
|
||||
static constexpr size_t GetSize() { return 16; }
|
||||
static constexpr std::string_view GetSchema() {
|
||||
return "double left;double right";
|
||||
}
|
||||
|
||||
static wpi::math::DifferentialDriveWheelAccelerations Unpack(
|
||||
std::span<const uint8_t> data);
|
||||
static void Pack(std::span<uint8_t> data,
|
||||
const wpi::math::DifferentialDriveWheelAccelerations& value);
|
||||
};
|
||||
|
||||
static_assert(wpi::util::StructSerializable<
|
||||
wpi::math::DifferentialDriveWheelAccelerations>);
|
||||
@@ -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<wpi::math::MecanumDriveWheelAccelerations> {
|
||||
static constexpr std::string_view GetTypeName() {
|
||||
return "MecanumDriveWheelAccelerations";
|
||||
}
|
||||
static constexpr size_t GetSize() { return 32; }
|
||||
static constexpr std::string_view GetSchema() {
|
||||
return "double front_left;double front_right;double rear_left;double "
|
||||
"rear_right";
|
||||
}
|
||||
|
||||
static wpi::math::MecanumDriveWheelAccelerations Unpack(
|
||||
std::span<const uint8_t> data);
|
||||
static void Pack(std::span<uint8_t> data,
|
||||
const wpi::math::MecanumDriveWheelAccelerations& value);
|
||||
};
|
||||
|
||||
static_assert(
|
||||
wpi::util::StructSerializable<wpi::math::MecanumDriveWheelAccelerations>);
|
||||
@@ -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<wpi::math::SwerveModuleAcceleration> {
|
||||
static constexpr std::string_view GetTypeName() {
|
||||
return "SwerveModuleAccelerations";
|
||||
}
|
||||
static constexpr size_t GetSize() {
|
||||
return 8 + wpi::util::GetStructSize<wpi::math::Rotation2d>();
|
||||
}
|
||||
static constexpr std::string_view GetSchema() {
|
||||
return "double acceleration;Rotation2d angle";
|
||||
}
|
||||
|
||||
static wpi::math::SwerveModuleAcceleration Unpack(
|
||||
std::span<const uint8_t> data);
|
||||
static void Pack(std::span<uint8_t> data,
|
||||
const wpi::math::SwerveModuleAcceleration& value);
|
||||
static void ForEachNested(
|
||||
std::invocable<std::string_view, std::string_view> auto fn) {
|
||||
wpi::util::ForEachStructSchema<wpi::math::Rotation2d>(fn);
|
||||
}
|
||||
};
|
||||
|
||||
static_assert(
|
||||
wpi::util::StructSerializable<wpi::math::SwerveModuleAcceleration>);
|
||||
static_assert(wpi::util::HasNestedStruct<wpi::math::SwerveModuleAcceleration>);
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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"]
|
||||
|
||||
@@ -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::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
|
||||
SwerveDrive3PoseEstimatorBase:
|
||||
qualname: wpi::math::PoseEstimator
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
|
||||
SwerveDrive4PoseEstimatorBase:
|
||||
qualname: wpi::math::PoseEstimator
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
|
||||
SwerveDrive6PoseEstimatorBase:
|
||||
qualname: wpi::math::PoseEstimator
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>
|
||||
|
||||
@@ -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::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
|
||||
SwerveDrive3PoseEstimator3dBase:
|
||||
qualname: wpi::math::PoseEstimator3d
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
|
||||
SwerveDrive4PoseEstimator3dBase:
|
||||
qualname: wpi::math::PoseEstimator3d
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
|
||||
SwerveDrive6PoseEstimator3dBase:
|
||||
qualname: wpi::math::PoseEstimator3d
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>
|
||||
|
||||
@@ -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==:
|
||||
@@ -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<wpi::math::DifferentialDriveKinematics>(cls_DifferentialDriveKinematics);
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
classes:
|
||||
wpi::math::DifferentialDriveWheelAccelerations:
|
||||
attributes:
|
||||
left:
|
||||
right:
|
||||
methods:
|
||||
operator+:
|
||||
operator-:
|
||||
overloads:
|
||||
const DifferentialDriveWheelAccelerations& [const]:
|
||||
'[const]':
|
||||
operator*:
|
||||
operator/:
|
||||
operator==:
|
||||
@@ -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::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
|
||||
SwerveDrive3KinematicsBase:
|
||||
qualname: wpi::math::Kinematics
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
|
||||
SwerveDrive4KinematicsBase:
|
||||
qualname: wpi::math::Kinematics
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
|
||||
SwerveDrive6KinematicsBase:
|
||||
qualname: wpi::math::Kinematics
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>
|
||||
|
||||
@@ -45,6 +45,11 @@ classes:
|
||||
GetRearLeft:
|
||||
GetRearRight:
|
||||
Interpolate:
|
||||
ToChassisAccelerations:
|
||||
ToWheelAccelerations:
|
||||
overloads:
|
||||
const ChassisAccelerations&, const Translation2d& [const]:
|
||||
const ChassisAccelerations& [const]:
|
||||
|
||||
inline_code: |
|
||||
SetupWPyStruct<wpi::math::MecanumDriveKinematics>(cls_MecanumDriveKinematics);
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
classes:
|
||||
wpi::math::MecanumDriveWheelAccelerations:
|
||||
attributes:
|
||||
frontLeft:
|
||||
frontRight:
|
||||
rearLeft:
|
||||
rearRight:
|
||||
methods:
|
||||
operator+:
|
||||
operator-:
|
||||
overloads:
|
||||
const MecanumDriveWheelAccelerations& [const]:
|
||||
'[const]':
|
||||
operator*:
|
||||
operator/:
|
||||
operator==:
|
||||
@@ -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::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
|
||||
SwerveDrive3OdometryBase:
|
||||
qualname: wpi::math::Odometry
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
|
||||
SwerveDrive4OdometryBase:
|
||||
qualname: wpi::math::Odometry
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
|
||||
SwerveDrive6OdometryBase:
|
||||
qualname: wpi::math::Odometry
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>
|
||||
|
||||
@@ -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::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,2>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,2>
|
||||
SwerveDrive3Odometry3dBase:
|
||||
qualname: wpi::math::Odometry3d
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,3>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,3>
|
||||
SwerveDrive4Odometry3dBase:
|
||||
qualname: wpi::math::Odometry3d
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,4>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,4>
|
||||
SwerveDrive6Odometry3dBase:
|
||||
qualname: wpi::math::Odometry3d
|
||||
params:
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModulePosition,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleState,6>
|
||||
- wpi::util::array<wpi::math::SwerveModuleAcceleration,6>
|
||||
|
||||
@@ -77,6 +77,13 @@ classes:
|
||||
|
||||
Interpolate:
|
||||
GetModules:
|
||||
ToSwerveModuleAccelerations:
|
||||
ToWheelAccelerations:
|
||||
ToChassisAccelerations:
|
||||
overloads:
|
||||
ModuleAccelerations&&... [const]:
|
||||
ignore: true
|
||||
const wpi::util::array<SwerveModuleAcceleration, NumModules>& [const]:
|
||||
|
||||
template_inline_code: |
|
||||
if constexpr (NumModules == 2) {
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
classes:
|
||||
wpi::math::SwerveModuleAcceleration:
|
||||
attributes:
|
||||
acceleration:
|
||||
angle:
|
||||
methods:
|
||||
operator==:
|
||||
operator+:
|
||||
operator-:
|
||||
overloads:
|
||||
const SwerveModuleAcceleration& [const]:
|
||||
'[const]':
|
||||
operator*:
|
||||
operator/:
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 <numbers>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
@@ -8,6 +8,7 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
|
||||
@@ -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 <gtest/gtest.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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 <gtest/gtest.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
@@ -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<Rotation2d, 4> 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<double, 4> expectedAccelerations;
|
||||
std::array<Rotation2d, 4> 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);
|
||||
}
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user