diff --git a/wpimath/src/main/python/semiwrap/kinematics/ChassisAccelerations.yml b/wpimath/src/main/python/semiwrap/kinematics/ChassisAccelerations.yml index d6eb353f70..e10b8074fc 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/ChassisAccelerations.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/ChassisAccelerations.yml @@ -1,5 +1,12 @@ +extra_includes: +- wpystruct.h + classes: wpi::math::ChassisAccelerations: + force_no_default_constructor: true + force_type_casters: + - wpi::units::meters_per_second_squared + - wpi::units::radians_per_second_squared_t attributes: ax: ay: @@ -15,3 +22,48 @@ classes: operator*: operator/: operator==: + +inline_code: | + cls_ChassisAccelerations + .def( + py::init< + wpi::units::meters_per_second_squared_t, wpi::units::meters_per_second_squared_t, + wpi::units::radians_per_second_squared_t + >(), + py::arg("ax") = 0, py::arg("ay") = 0, py::arg("alpha") = 0 + ) + .def_static("fromFps", [](wpi::units::feet_per_second_squared_t ax, wpi::units::feet_per_second_squared_t ay, wpi::units::radians_per_second_squared_t alpha){ + return ChassisAccelerations{ax, ay, alpha}; + }, py::arg("ax") = 0, py::arg("ay") = 0, py::arg("alpha") = 0) + .def_property("ax_fpss", + [](ChassisAccelerations * self) -> wpi::units::feet_per_second_squared_t { + return self->ax; + }, + [](ChassisAccelerations * self, wpi::units::feet_per_second_squared_t ax) { + self->ax = ax; + } + ) + .def_property("ay_fpss", + [](ChassisAccelerations * self) -> wpi::units::feet_per_second_squared_t { + return self->ay; + }, + [](ChassisAccelerations * self, wpi::units::feet_per_second_squared_t ay) { + self->ay = ay; + } + ) + .def_property("alpha_dpss", + [](ChassisAccelerations * self) -> wpi::units::degrees_per_second_squared_t { + return self->alpha; + }, + [](ChassisAccelerations * self, wpi::units::degrees_per_second_squared_t alpha) { + self->alpha = alpha; + } + ) + .def("__repr__", [](const ChassisAccelerations &ca) -> std::string { + return "ChassisAccelerations(ax=" + std::to_string(ca.ax()) + ", " + "ay=" + std::to_string(ca.ay()) + ", " + "alpha=" + std::to_string(ca.alpha()) + ")"; + }) + ; + + SetupWPyStruct(cls_ChassisAccelerations); diff --git a/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveWheelAccelerations.yml b/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveWheelAccelerations.yml index 47c41a4f0b..8b5b139ebd 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveWheelAccelerations.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/DifferentialDriveWheelAccelerations.yml @@ -1,5 +1,11 @@ +extra_includes: +- wpystruct.h + classes: wpi::math::DifferentialDriveWheelAccelerations: + force_no_default_constructor: true + force_type_casters: + - wpi::units::meters_per_second_squared attributes: left: right: @@ -12,3 +18,36 @@ classes: operator*: operator/: operator==: + +inline_code: | + cls_DifferentialDriveWheelAccelerations + .def( + py::init(), + py::arg("left") = 0, py::arg("right") = 0 + ) + .def_static("fromFps", [](wpi::units::feet_per_second_squared_t left, wpi::units::feet_per_second_squared_t right){ + return DifferentialDriveWheelAccelerations{left, right}; + }, py::arg("left"), py::arg("right")) + .def_property("left_fpss", + [](DifferentialDriveWheelAccelerations * self) -> wpi::units::feet_per_second_squared_t { + return self->left; + }, + [](DifferentialDriveWheelAccelerations * self, wpi::units::feet_per_second_squared_t left) { + self->left = left; + } + ) + .def_property("right_fpss", + [](DifferentialDriveWheelAccelerations * self) -> wpi::units::feet_per_second_squared_t { + return self->right; + }, + [](DifferentialDriveWheelAccelerations * self, wpi::units::feet_per_second_squared_t right) { + self->right = right; + } + ) + .def("__repr__", [](const DifferentialDriveWheelAccelerations &dda) -> std::string { + return "DifferentialDriveWheelAccelerations(left=" + std::to_string(dda.left()) + ", " + "right=" + std::to_string(dda.right()) + ")"; + }) + ; + + SetupWPyStruct(cls_DifferentialDriveWheelAccelerations); diff --git a/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveWheelAccelerations.yml b/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveWheelAccelerations.yml index f69a33d8b1..58c99dcf47 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveWheelAccelerations.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/MecanumDriveWheelAccelerations.yml @@ -1,5 +1,11 @@ +extra_includes: +- wpystruct.h + classes: wpi::math::MecanumDriveWheelAccelerations: + force_no_default_constructor: true + force_type_casters: + - wpi::units::meters_per_second_squared attributes: frontLeft: frontRight: @@ -14,3 +20,59 @@ classes: operator*: operator/: operator==: + +inline_code: | + cls_MecanumDriveWheelAccelerations + .def( + py::init< + wpi::units::meters_per_second_squared_t, wpi::units::meters_per_second_squared_t, wpi::units::meters_per_second_squared_t, wpi::units::meters_per_second_squared_t + >(), + py::arg("frontLeft") = 0, py::arg("frontRight") = 0, py::arg("rearLeft") = 0, py::arg("rearRight") = 0 + ) + .def_static("fromFps", [](wpi::units::feet_per_second_squared_t frontLeft, + wpi::units::feet_per_second_squared_t frontRight, + wpi::units::feet_per_second_squared_t rearLeft, + wpi::units::feet_per_second_squared_t rearRight){ + return MecanumDriveWheelAccelerations{frontLeft, frontRight, rearLeft, rearRight}; + }, py::arg("frontLeft") = 0, py::arg("frontRight") = 0, py::arg("rearLeft") = 0, py::arg("rearRight") = 0) + .def_property("front_left_fpss", + [](MecanumDriveWheelAccelerations * self) -> wpi::units::feet_per_second_squared_t { + return self->frontLeft; + }, + [](MecanumDriveWheelAccelerations * self, wpi::units::feet_per_second_squared_t frontLeft) { + self->frontLeft = frontLeft; + } + ) + .def_property("front_right_fpss", + [](MecanumDriveWheelAccelerations * self) -> wpi::units::feet_per_second_squared_t { + return self->frontRight; + }, + [](MecanumDriveWheelAccelerations * self, wpi::units::feet_per_second_squared_t frontRight) { + self->frontRight = frontRight; + } + ) + .def_property("rear_left_fpss", + [](MecanumDriveWheelAccelerations * self) -> wpi::units::feet_per_second_squared_t { + return self->rearLeft; + }, + [](MecanumDriveWheelAccelerations * self, wpi::units::feet_per_second_squared_t rearLeft) { + self->rearLeft = rearLeft; + } + ) + .def_property("rear_right_fpss", + [](MecanumDriveWheelAccelerations * self) -> wpi::units::feet_per_second_squared_t { + return self->rearRight; + }, + [](MecanumDriveWheelAccelerations * self, wpi::units::feet_per_second_squared_t rearRight) { + self->rearRight = rearRight; + } + ) + .def("__repr__", [](const MecanumDriveWheelAccelerations &mdwa) -> std::string { + return "MecanumDriveWheelAccelerations(frontLeft=" + std::to_string(mdwa.frontLeft()) + ", " + "frontRight=" + std::to_string(mdwa.frontRight()) + ", " + "rearLeft=" + std::to_string(mdwa.rearLeft()) + ", " + "rearRight=" + std::to_string(mdwa.rearRight()) + ")"; + }) + ; + + SetupWPyStruct(cls_MecanumDriveWheelAccelerations); diff --git a/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAcceleration.yml b/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAcceleration.yml index 3757525732..7801b10744 100644 --- a/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAcceleration.yml +++ b/wpimath/src/main/python/semiwrap/kinematics/SwerveModuleAcceleration.yml @@ -1,5 +1,11 @@ +extra_includes: +- wpystruct.h + classes: wpi::math::SwerveModuleAcceleration: + force_no_default_constructor: true + force_type_casters: + - wpi::units::meters_per_second_squared attributes: acceleration: angle: @@ -12,3 +18,30 @@ classes: '[const]': operator*: operator/: + +inline_code: | + cls_SwerveModuleAcceleration + .def( + py::init< + wpi::units::meters_per_second_squared_t, wpi::math::Rotation2d + >(), + py::arg("acceleration") = 0, py::arg("angle") = wpi::math::Rotation2d() + ) + .def_static("fromFps", [](wpi::units::feet_per_second_squared_t acceleration, wpi::math::Rotation2d angle){ + return SwerveModuleAcceleration{acceleration, angle}; + }, py::arg("acceleration") = 0, py::arg("angle") = wpi::math::Rotation2d()) + .def_property("acceleration_fpss", + [](SwerveModuleAcceleration * self) -> wpi::units::feet_per_second_squared_t { + return self->acceleration; + }, + [](SwerveModuleAcceleration * self, wpi::units::feet_per_second_squared_t acceleration) { + self->acceleration = acceleration; + } + ) + .def("__repr__", [](const SwerveModuleAcceleration &sma) -> std::string { + return "SwerveModuleAcceleration(acceleration=" + std::to_string(sma.acceleration()) + ", " + "angle=" + std::to_string(sma.angle.Radians()()) + ")"; + }) + ; + + SetupWPyStruct(cls_SwerveModuleAcceleration); diff --git a/wpimath/src/main/python/wpimath/kinematics/__init__.py b/wpimath/src/main/python/wpimath/kinematics/__init__.py index f2c7f29aab..155495e95b 100644 --- a/wpimath/src/main/python/wpimath/kinematics/__init__.py +++ b/wpimath/src/main/python/wpimath/kinematics/__init__.py @@ -2,6 +2,7 @@ from . import _init__kinematics # autogenerated by 'semiwrap create-imports wpimath.kinematics wpimath.kinematics._kinematics' from ._kinematics import ( + ChassisAccelerations, ChassisSpeeds, DifferentialDriveKinematics, DifferentialDriveKinematicsBase, @@ -9,6 +10,7 @@ from ._kinematics import ( DifferentialDriveOdometry3d, DifferentialDriveOdometry3dBase, DifferentialDriveOdometryBase, + DifferentialDriveWheelAccelerations, DifferentialDriveWheelPositions, DifferentialDriveWheelSpeeds, MecanumDriveKinematics, @@ -17,6 +19,7 @@ from ._kinematics import ( MecanumDriveOdometry3d, MecanumDriveOdometry3dBase, MecanumDriveOdometryBase, + MecanumDriveWheelAccelerations, MecanumDriveWheelPositions, MecanumDriveWheelSpeeds, SwerveDrive2Kinematics, @@ -43,11 +46,13 @@ from ._kinematics import ( SwerveDrive6Odometry3d, SwerveDrive6Odometry3dBase, SwerveDrive6OdometryBase, + SwerveModuleAcceleration, SwerveModulePosition, SwerveModuleState, ) __all__ = [ + "ChassisAccelerations", "ChassisSpeeds", "DifferentialDriveKinematics", "DifferentialDriveKinematicsBase", @@ -55,6 +60,7 @@ __all__ = [ "DifferentialDriveOdometry3d", "DifferentialDriveOdometry3dBase", "DifferentialDriveOdometryBase", + "DifferentialDriveWheelAccelerations", "DifferentialDriveWheelPositions", "DifferentialDriveWheelSpeeds", "MecanumDriveKinematics", @@ -63,6 +69,7 @@ __all__ = [ "MecanumDriveOdometry3d", "MecanumDriveOdometry3dBase", "MecanumDriveOdometryBase", + "MecanumDriveWheelAccelerations", "MecanumDriveWheelPositions", "MecanumDriveWheelSpeeds", "SwerveDrive2Kinematics", @@ -89,6 +96,7 @@ __all__ = [ "SwerveDrive6Odometry3d", "SwerveDrive6Odometry3dBase", "SwerveDrive6OdometryBase", + "SwerveModuleAcceleration", "SwerveModulePosition", "SwerveModuleState", ] diff --git a/wpimath/src/test/python/kinematics/test_chassis_accelerations.py b/wpimath/src/test/python/kinematics/test_chassis_accelerations.py new file mode 100644 index 0000000000..b615575cd9 --- /dev/null +++ b/wpimath/src/test/python/kinematics/test_chassis_accelerations.py @@ -0,0 +1,119 @@ +import math +import pytest +from wpimath.kinematics import ChassisAccelerations +from wpimath.geometry import Rotation2d +from wpimath.units import feetToMeters + +K_EPSILON = 1e-9 + + +def test_default_constructor(): + accelerations = ChassisAccelerations() + + assert accelerations.ax == pytest.approx(0.0, abs=K_EPSILON) + assert accelerations.ay == pytest.approx(0.0, abs=K_EPSILON) + assert accelerations.alpha == pytest.approx(0.0, abs=K_EPSILON) + + +def test_parameterized_constructor(): + # 1.0 m/s^2, 2.0 m/s^2, 3.0 rad/s^2 + accelerations = ChassisAccelerations(1.0, 2.0, 3.0) + + assert accelerations.ax == pytest.approx(1.0, abs=K_EPSILON) + assert accelerations.ay == pytest.approx(2.0, abs=K_EPSILON) + assert accelerations.alpha == pytest.approx(3.0, abs=K_EPSILON) + + +def test_to_robot_relative(): + start_accel = ChassisAccelerations(1.0, 0.0, 0.5) + + chassis_accelerations = start_accel.toRobotRelative(Rotation2d.fromDegrees(-90)) + + assert chassis_accelerations.ax == pytest.approx(0.0, abs=K_EPSILON) + assert chassis_accelerations.ay == pytest.approx(1.0, abs=K_EPSILON) + assert chassis_accelerations.alpha == pytest.approx(0.5, abs=K_EPSILON) + + +def test_to_field_relative(): + start_accel = ChassisAccelerations(1.0, 0.0, 0.5) + + chassis_accelerations = start_accel.toFieldRelative(Rotation2d.fromDegrees(45)) + + expected_val = 1.0 / math.sqrt(2.0) + + assert chassis_accelerations.ax == pytest.approx(expected_val, abs=K_EPSILON) + assert chassis_accelerations.ay == pytest.approx(expected_val, abs=K_EPSILON) + assert chassis_accelerations.alpha == pytest.approx(0.5, abs=K_EPSILON) + + +def test_plus(): + left = ChassisAccelerations(1.0, 0.5, 0.75) + right = ChassisAccelerations(2.0, 1.5, 0.25) + + chassis_accelerations = left + right + + assert chassis_accelerations.ax == pytest.approx(3.0, abs=K_EPSILON) + assert chassis_accelerations.ay == pytest.approx(2.0, abs=K_EPSILON) + assert chassis_accelerations.alpha == pytest.approx(1.0, abs=K_EPSILON) + + +def test_minus(): + left = ChassisAccelerations(1.0, 0.5, 0.75) + right = ChassisAccelerations(2.0, 0.5, 0.25) + + chassis_accelerations = left - right + + assert chassis_accelerations.ax == pytest.approx(-1.0, abs=K_EPSILON) + assert chassis_accelerations.ay == pytest.approx(0.0, abs=K_EPSILON) + assert chassis_accelerations.alpha == pytest.approx(0.5, abs=K_EPSILON) + + +def test_unary_minus(): + accel = ChassisAccelerations(1.0, 0.5, 0.75) + + chassis_accelerations = -accel + + assert chassis_accelerations.ax == pytest.approx(-1.0, abs=K_EPSILON) + assert chassis_accelerations.ay == pytest.approx(-0.5, abs=K_EPSILON) + assert chassis_accelerations.alpha == pytest.approx(-0.75, abs=K_EPSILON) + + +def test_multiplication(): + accel = ChassisAccelerations(1.0, 0.5, 0.75) + + chassis_accelerations = accel * 2.0 + + assert chassis_accelerations.ax == pytest.approx(2.0, abs=K_EPSILON) + assert chassis_accelerations.ay == pytest.approx(1.0, abs=K_EPSILON) + assert chassis_accelerations.alpha == pytest.approx(1.5, abs=K_EPSILON) + + +def test_division(): + accel = ChassisAccelerations(2.0, 1.0, 1.5) + + chassis_accelerations = accel / 2.0 + + assert chassis_accelerations.ax == pytest.approx(1.0, abs=K_EPSILON) + assert chassis_accelerations.ay == pytest.approx(0.5, abs=K_EPSILON) + assert chassis_accelerations.alpha == pytest.approx(0.75, abs=K_EPSILON) + + +def test_feet_constructor(): + chassis_accelerations = ChassisAccelerations.fromFps(10, 11, math.radians(45)) + + assert chassis_accelerations.ax == pytest.approx(feetToMeters(10), abs=K_EPSILON) + assert chassis_accelerations.ay == pytest.approx(feetToMeters(11), abs=K_EPSILON) + assert chassis_accelerations.alpha == pytest.approx(math.radians(45), abs=K_EPSILON) + + assert chassis_accelerations.ax_fpss == pytest.approx(10, abs=K_EPSILON) + assert chassis_accelerations.ay_fpss == pytest.approx(11, abs=K_EPSILON) + assert chassis_accelerations.alpha_dpss == pytest.approx(45, abs=K_EPSILON) + + +def test_repr(): + chassis_accelerations = ChassisAccelerations(1, 2, 3) + + assert ( + str(chassis_accelerations) + == "ChassisAccelerations(ax=1.000000, ay=2.000000, alpha=3.000000)" + ) diff --git a/wpimath/src/test/python/kinematics/test_differential_drive_wheel_accelerations.py b/wpimath/src/test/python/kinematics/test_differential_drive_wheel_accelerations.py new file mode 100644 index 0000000000..fabae750b2 --- /dev/null +++ b/wpimath/src/test/python/kinematics/test_differential_drive_wheel_accelerations.py @@ -0,0 +1,85 @@ +import pytest +from wpimath.kinematics import DifferentialDriveWheelAccelerations +from wpimath.units import feetToMeters + +K_EPSILON = 1e-9 + + +def test_default_constructor(): + wheel_accelerations = DifferentialDriveWheelAccelerations() + + assert wheel_accelerations.left == pytest.approx(0.0, abs=K_EPSILON) + assert wheel_accelerations.right == pytest.approx(0.0, abs=K_EPSILON) + + +def test_parameterized_constructor(): + wheel_accelerations = DifferentialDriveWheelAccelerations(1.5, 2.5) + + assert wheel_accelerations.left == pytest.approx(1.5, abs=K_EPSILON) + assert wheel_accelerations.right == pytest.approx(2.5, abs=K_EPSILON) + + +def test_plus(): + left = DifferentialDriveWheelAccelerations(1.0, 0.5) + right = DifferentialDriveWheelAccelerations(2.0, 1.5) + + wheel_accelerations = left + right + + assert wheel_accelerations.left == pytest.approx(3.0, abs=K_EPSILON) + assert wheel_accelerations.right == pytest.approx(2.0, abs=K_EPSILON) + + +def test_minus(): + left = DifferentialDriveWheelAccelerations(1.0, 0.5) + right = DifferentialDriveWheelAccelerations(2.0, 0.5) + + wheel_accelerations = left - right + + assert wheel_accelerations.left == pytest.approx(-1.0, abs=K_EPSILON) + assert wheel_accelerations.right == pytest.approx(0.0, abs=K_EPSILON) + + +def test_unary_minus(): + accel = DifferentialDriveWheelAccelerations(1.0, 0.5) + + wheel_accelerations = -accel + + assert wheel_accelerations.left == pytest.approx(-1.0, abs=K_EPSILON) + assert wheel_accelerations.right == pytest.approx(-0.5, abs=K_EPSILON) + + +def test_multiplication(): + accel = DifferentialDriveWheelAccelerations(1.0, 0.5) + + wheel_accelerations = accel * 2.0 + + assert wheel_accelerations.left == pytest.approx(2.0, abs=K_EPSILON) + assert wheel_accelerations.right == pytest.approx(1.0, abs=K_EPSILON) + + +def test_division(): + accel = DifferentialDriveWheelAccelerations(1.0, 0.5) + + wheel_accelerations = accel / 2.0 + + assert wheel_accelerations.left == pytest.approx(0.5, abs=K_EPSILON) + assert wheel_accelerations.right == pytest.approx(0.25, abs=K_EPSILON) + + +def test_feet_constructor(): + accel = DifferentialDriveWheelAccelerations.fromFps(10, 11) + + assert accel.left == pytest.approx(feetToMeters(10), abs=K_EPSILON) + assert accel.right == pytest.approx(feetToMeters(11), abs=K_EPSILON) + + assert accel.left_fpss == pytest.approx(10, abs=K_EPSILON) + assert accel.right_fpss == pytest.approx(11, abs=K_EPSILON) + + +def test_repr(): + accel = DifferentialDriveWheelAccelerations(1, 2) + + assert ( + str(accel) + == "DifferentialDriveWheelAccelerations(left=1.000000, right=2.000000)" + ) diff --git a/wpimath/src/test/python/kinematics/test_mecanum_drive_wheel_accelerations.py b/wpimath/src/test/python/kinematics/test_mecanum_drive_wheel_accelerations.py new file mode 100644 index 0000000000..c26e8b7207 --- /dev/null +++ b/wpimath/src/test/python/kinematics/test_mecanum_drive_wheel_accelerations.py @@ -0,0 +1,105 @@ +import pytest +from wpimath.kinematics import MecanumDriveWheelAccelerations +from wpimath.units import feetToMeters + +# Epsilon matching the C++ test +K_EPSILON = 1e-9 + + +def test_default_constructor(): + wheel_accelerations = MecanumDriveWheelAccelerations() + + assert wheel_accelerations.frontLeft == pytest.approx(0.0, abs=K_EPSILON) + assert wheel_accelerations.frontRight == pytest.approx(0.0, abs=K_EPSILON) + assert wheel_accelerations.rearLeft == pytest.approx(0.0, abs=K_EPSILON) + assert wheel_accelerations.rearRight == pytest.approx(0.0, abs=K_EPSILON) + + +def test_parameterized_constructor(): + # Accelerations: FL=1.0, FR=2.0, RL=3.0, RR=4.0 m/s^2 + wheel_accelerations = MecanumDriveWheelAccelerations(1.0, 2.0, 3.0, 4.0) + + assert wheel_accelerations.frontLeft == pytest.approx(1.0, abs=K_EPSILON) + assert wheel_accelerations.frontRight == pytest.approx(2.0, abs=K_EPSILON) + assert wheel_accelerations.rearLeft == pytest.approx(3.0, abs=K_EPSILON) + assert wheel_accelerations.rearRight == pytest.approx(4.0, abs=K_EPSILON) + + +def test_plus(): + left = MecanumDriveWheelAccelerations(1.0, 0.5, 0.75, 0.25) + right = MecanumDriveWheelAccelerations(1.0, 1.5, 0.25, 1.75) + + wheel_accelerations = left + right + + assert wheel_accelerations.frontLeft == pytest.approx(2.0, abs=K_EPSILON) + assert wheel_accelerations.frontRight == pytest.approx(2.0, abs=K_EPSILON) + assert wheel_accelerations.rearLeft == pytest.approx(1.0, abs=K_EPSILON) + assert wheel_accelerations.rearRight == pytest.approx(2.0, abs=K_EPSILON) + + +def test_minus(): + left = MecanumDriveWheelAccelerations(1.0, 0.5, 0.75, 0.25) + right = MecanumDriveWheelAccelerations(2.0, 1.5, 0.25, 0.75) + + wheel_accelerations = left - right + + assert wheel_accelerations.frontLeft == pytest.approx(-1.0, abs=K_EPSILON) + assert wheel_accelerations.frontRight == pytest.approx(-1.0, abs=K_EPSILON) + assert wheel_accelerations.rearLeft == pytest.approx(0.5, abs=K_EPSILON) + assert wheel_accelerations.rearRight == pytest.approx(-0.5, abs=K_EPSILON) + + +def test_unary_minus(): + accel = MecanumDriveWheelAccelerations(1.0, 0.5, 0.75, 0.25) + + wheel_accelerations = -accel + + assert wheel_accelerations.frontLeft == pytest.approx(-1.0, abs=K_EPSILON) + assert wheel_accelerations.frontRight == pytest.approx(-0.5, abs=K_EPSILON) + assert wheel_accelerations.rearLeft == pytest.approx(-0.75, abs=K_EPSILON) + assert wheel_accelerations.rearRight == pytest.approx(-0.25, abs=K_EPSILON) + + +def test_multiplication(): + accel = MecanumDriveWheelAccelerations(1.0, 0.5, 0.75, 0.25) + + wheel_accelerations = accel * 2.0 + + assert wheel_accelerations.frontLeft == pytest.approx(2.0, abs=K_EPSILON) + assert wheel_accelerations.frontRight == pytest.approx(1.0, abs=K_EPSILON) + assert wheel_accelerations.rearLeft == pytest.approx(1.5, abs=K_EPSILON) + assert wheel_accelerations.rearRight == pytest.approx(0.5, abs=K_EPSILON) + + +def test_division(): + accel = MecanumDriveWheelAccelerations(2.0, 1.0, 1.5, 0.5) + + wheel_accelerations = accel / 2.0 + + assert wheel_accelerations.frontLeft == pytest.approx(1.0, abs=K_EPSILON) + assert wheel_accelerations.frontRight == pytest.approx(0.5, abs=K_EPSILON) + assert wheel_accelerations.rearLeft == pytest.approx(0.75, abs=K_EPSILON) + assert wheel_accelerations.rearRight == pytest.approx(0.25, abs=K_EPSILON) + + +def test_feet_constructor(): + accel = MecanumDriveWheelAccelerations.fromFps(10, 11, 12, 13) + + assert accel.frontLeft == pytest.approx(feetToMeters(10), abs=K_EPSILON) + assert accel.frontRight == pytest.approx(feetToMeters(11), abs=K_EPSILON) + assert accel.rearLeft == pytest.approx(feetToMeters(12), abs=K_EPSILON) + assert accel.rearRight == pytest.approx(feetToMeters(13), abs=K_EPSILON) + + assert accel.front_left_fpss == pytest.approx(10, abs=K_EPSILON) + assert accel.front_right_fpss == pytest.approx(11, abs=K_EPSILON) + assert accel.rear_left_fpss == pytest.approx(12, abs=K_EPSILON) + assert accel.rear_right_fpss == pytest.approx(13, abs=K_EPSILON) + + +def test_repr(): + accel = MecanumDriveWheelAccelerations(1, 2, 3, 4) + + assert ( + str(accel) + == "MecanumDriveWheelAccelerations(frontLeft=1.000000, frontRight=2.000000, rearLeft=3.000000, rearRight=4.000000)" + ) diff --git a/wpimath/src/test/python/kinematics/test_swerve_module_accelerations.py b/wpimath/src/test/python/kinematics/test_swerve_module_accelerations.py new file mode 100644 index 0000000000..d0535af515 --- /dev/null +++ b/wpimath/src/test/python/kinematics/test_swerve_module_accelerations.py @@ -0,0 +1,48 @@ +import pytest +import math +from wpimath.kinematics import SwerveModuleAcceleration +from wpimath.geometry import Rotation2d +from wpimath.units import feetToMeters + +K_EPSILON = 1e-9 + + +def test_default_constructor(): + module_accelerations = SwerveModuleAcceleration() + + assert module_accelerations.acceleration == pytest.approx(0.0, abs=K_EPSILON) + assert module_accelerations.angle.radians() == pytest.approx(0.0, abs=K_EPSILON) + + +def test_parameterized_constructor(): + module_accelerations = SwerveModuleAcceleration(2.5, Rotation2d(1.5)) + + assert module_accelerations.acceleration == pytest.approx(2.5, abs=K_EPSILON) + assert module_accelerations.angle.radians() == pytest.approx(1.5, abs=K_EPSILON) + + +def test_equals(): + module_accelerations1 = SwerveModuleAcceleration(2.0, Rotation2d(1.5)) + module_accelerations2 = SwerveModuleAcceleration(2.0, Rotation2d(1.5)) + + module_accelerations3 = SwerveModuleAcceleration(2.1, Rotation2d(1.5)) + + assert module_accelerations1 == module_accelerations2 + assert module_accelerations1 != module_accelerations3 + + +def test_feet_constructor(): + accel = SwerveModuleAcceleration.fromFps(10, Rotation2d(1.5)) + + assert accel.acceleration == pytest.approx(feetToMeters(10), abs=K_EPSILON) + assert accel.angle.radians() == pytest.approx(1.5, abs=K_EPSILON) + + assert accel.acceleration_fpss == pytest.approx(10, abs=K_EPSILON) + + +def test_repr(): + accel = SwerveModuleAcceleration(1, Rotation2d(1.5)) + + assert ( + str(accel) == "SwerveModuleAcceleration(acceleration=1.000000, angle=1.500000)" + )