[robotpy] Mirror most other subprojects (#8208)

GitOrigin-RevId: ac60fd3cf4a24023184376687da28373d14b781a

This mirrors the robotpy files for the following projects:
- apriltag
- datalog
- hal
- ntcore
- romiVendordep
- wpilibc
- wpimath
- xrpVendordep

This excludes cscore and the halsim wrappers for at this time.

NOTE: This does not hook these projects up to the build system, just simply mirrors the files. The building will take place in a follow up PR to make it easier to review the changes necessary to build.

wpimath/src/test/python/kinematics/test_chassis_speeds.py

@@ -0,0 +1,72 @@
+import math
+
+from wpimath.geometry import Rotation2d
+from wpimath.kinematics import ChassisSpeeds
+
+
+def test_plus() -> None:
+    left = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
+    right = ChassisSpeeds(vx=2.0, vy=1.5, omega=0.25)
+
+    result = left + right
+
+    assert math.isclose(3.0, result.vx)
+    assert math.isclose(2.0, result.vy)
+    assert math.isclose(1.0, result.omega)
+
+
+def test_minus() -> None:
+    left = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
+    right = ChassisSpeeds(vx=2.0, vy=0.5, omega=0.25)
+
+    result = left - right
+
+    assert math.isclose(-1.0, result.vx)
+    assert math.isclose(0, result.vy)
+    assert math.isclose(0.5, result.omega)
+
+
+def test_neg() -> None:
+    speeds = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
+
+    result = -speeds
+
+    assert math.isclose(-1.0, result.vx)
+    assert math.isclose(-0.5, result.vy)
+    assert math.isclose(-0.75, result.omega)
+
+
+def test_multiplication() -> None:
+    speeds = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
+
+    result = speeds * 2
+
+    assert math.isclose(2.0, result.vx)
+    assert math.isclose(1.0, result.vy)
+    assert math.isclose(1.5, result.omega)
+
+
+def test_division() -> None:
+    speeds = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
+
+    result = speeds / 2
+
+    assert math.isclose(0.5, result.vx)
+    assert math.isclose(0.25, result.vy)
+    assert math.isclose(0.375, result.omega)
+
+
+def test_unpack() -> None:
+    speeds = ChassisSpeeds(1, 1, 0.5)
+
+    vx, vy, omega = speeds
+
+    assert math.isclose(1, vx)
+    assert math.isclose(1, vy)
+    assert math.isclose(0.5, omega)
+
+
+def test_repr() -> None:
+    speeds = ChassisSpeeds(2.0, 1.0, 0.0)
+
+    assert repr(speeds) == "ChassisSpeeds(vx=2.000000, vy=1.000000, omega=0.000000)"

wpimath/src/test/python/kinematics/test_swerve.py

@@ -0,0 +1,75 @@
+import pytest
+
+from wpimath.geometry import Pose2d, Rotation2d, Translation2d
+from wpimath.kinematics import (
+    ChassisSpeeds,
+    SwerveDrive4Kinematics,
+    SwerveDrive4Odometry,
+    SwerveModulePosition,
+    SwerveModuleState,
+)
+
+
+@pytest.fixture()
+def s4():
+    fl = Translation2d(12, 12)
+    fr = Translation2d(12, -12)
+    bl = Translation2d(-12, 12)
+    br = Translation2d(-12, -12)
+    return SwerveDrive4Kinematics(fl, fr, bl, br)
+
+
+def test_swerve4_straightline(s4: SwerveDrive4Kinematics):
+    chassisSpeeds = ChassisSpeeds(5, 0, 0)
+
+    fl, fr, bl, br = s4.toSwerveModuleStates(chassisSpeeds)
+    assert fl.speed == pytest.approx(5.0)
+    assert fr.speed == pytest.approx(5.0)
+    assert bl.speed == pytest.approx(5.0)
+    assert br.speed == pytest.approx(5.0)
+
+    assert fl.angle.radians() == pytest.approx(0.0)
+    assert fr.angle.radians() == pytest.approx(0.0)
+    assert bl.angle.radians() == pytest.approx(0.0)
+    assert br.angle.radians() == pytest.approx(0.0)
+
+
+def test_swerve4_normalize():
+    s1 = SwerveModuleState(5)
+    s2 = SwerveModuleState(6)
+    s3 = SwerveModuleState(4)
+    s4 = SwerveModuleState(7)
+
+    states = SwerveDrive4Kinematics.desaturateWheelSpeeds((s1, s2, s3, s4), 5.5)
+
+    kFactor = 5.5 / 7.0
+
+    assert states[0].speed == pytest.approx(5.0 * kFactor)
+    assert states[1].speed == pytest.approx(6.0 * kFactor)
+    assert states[2].speed == pytest.approx(4.0 * kFactor)
+    assert states[3].speed == pytest.approx(7.0 * kFactor)
+
+
+def test_swerve4_odometry(s4: SwerveDrive4Kinematics):
+    zero = SwerveModulePosition()
+    odometry = SwerveDrive4Odometry(s4, Rotation2d(0), (zero, zero, zero, zero))
+    odometry.resetPosition(Rotation2d(0), (zero, zero, zero, zero), Pose2d())
+
+    position = SwerveModulePosition(0.5)
+
+    odometry.update(
+        Rotation2d(0),
+        (
+            zero,
+            zero,
+            zero,
+            zero,
+        ),
+    )
+
+    pose = odometry.update(Rotation2d(0), (position, position, position, position))
+
+    print(pose)
+    assert pose.x == pytest.approx(0.5)
+    assert pose.y == pytest.approx(0.0)
+    assert pose.rotation().degrees() == pytest.approx(0)