[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/conftest.py

@@ -0,0 +1 @@
+

wpimath/src/test/python/cpp/pyproject.toml

@@ -0,0 +1,27 @@
+[build-system]
+build-backend = "hatchling.build"
+requires = [
+    "semiwrap", "hatch-meson", "hatchling",
+]
+
+[project]
+name = "wpimath_test"
+version = "0.1"
+description = "Test project for verifying robotpy-build behavior"
+authors = [
+    {name = "RobotPy Development Team", email = "robotpy@googlegroups.com"},
+]
+license = "BSD-3-Clause"
+
+[tool.hatch.build.hooks.semiwrap]
+[tool.hatch.build.hooks.meson]
+
+[tool.semiwrap]
+
+[tool.semiwrap.extension_modules."wpimath_test._wpimath_test"]
+name = "wpimath_test"
+depends = ["wpimath"]
+includes = ["wpimath_test/include"]
+
+[tool.semiwrap.extension_modules."wpimath_test._wpimath_test".headers]
+module = "module.h"

wpimath/src/test/python/cpp/semiwrap/module.yml

@@ -0,0 +1,19 @@
+---
+
+classes:
+  SomeClass:
+    attributes:
+      kDefaultPeriod:
+      five_ft:
+    methods:
+      checkDefaultByName:
+      checkDefaultByNum1:
+        param_override:
+          period:
+            default: 50_ms
+      checkDefaultByNum2:
+        param_override:
+          period:
+            default: 0.050_s
+      ms2s:
+      ft2m:

wpimath/src/test/python/cpp/wpimath_test/__init__.py

@@ -0,0 +1,8 @@
+from . import _init__wpimath_test
+
+# autogenerated by 'robotpy-build create-imports wpimath_test'
+from ._wpimath_test import SomeClass
+
+__all__ = ["SomeClass"]
+
+del _init__wpimath_test

wpimath/src/test/python/cpp/wpimath_test/include/module.h

@@ -0,0 +1,24 @@
+
+#pragma once
+
+#include <units/time.h>
+#include <units/length.h>
+
+struct SomeClass {
+    static constexpr auto s_constant = 2_s;
+    static constexpr auto ms_constant1 = 20_ms;
+    static constexpr units::second_t ms_constant2 = 50_ms;
+    static constexpr units::millisecond_t ms_constant3 = 0.20_s;
+
+    bool checkDefaultByName1(units::second_t period = ms_constant1);
+    bool checkDefaultByName2(units::second_t period = ms_constant2);
+    bool checkDefaultByNum1(units::second_t period = 50_ms);
+    bool checkDefaultByNum2(units::second_t period = 0.5_s);
+
+    units::second_t ms2s(units::millisecond_t ms);
+    units::millisecond_t s2ms(units::second_t s);
+
+    static constexpr units::foot_t five_ft = 5_ft;
+
+    units::meter_t ft2m(units::foot_t f);
+};

wpimath/src/test/python/cpp/wpimath_test/src/module.cpp

@@ -0,0 +1,53 @@
+
+#include "semiwrap_init.wpimath_test._wpimath_test.hpp"
+#include <module.h>
+#include <stdexcept>
+
+SEMIWRAP_PYBIND11_MODULE(m)
+{
+    initWrapper(m);
+}
+
+bool SomeClass::checkDefaultByName1(units::second_t period)
+{
+    if (period != SomeClass::ms_constant1) {
+        throw std::runtime_error(units::to_string(period));
+    }
+    return true;
+}
+
+bool SomeClass::checkDefaultByName2(units::second_t period)
+{
+    if (period != SomeClass::ms_constant2) {
+        throw std::runtime_error(units::to_string(period));
+    }
+    return true;
+}
+
+bool SomeClass::checkDefaultByNum1(units::second_t period)
+{
+    if (period != 50_ms) {
+        throw std::runtime_error(units::to_string(period));
+    }
+    return true;
+}
+
+bool SomeClass::checkDefaultByNum2(units::second_t period)
+{
+    if (period != 50_ms) {
+        throw std::runtime_error(units::to_string(period));
+    }
+    return true;
+}
+
+units::meter_t SomeClass::ft2m(units::foot_t f) {
+    return f;
+}
+
+units::second_t SomeClass::ms2s(units::millisecond_t ms) {
+    return ms;
+}
+
+units::millisecond_t SomeClass::s2ms(units::second_t s) {
+    return s;
+}

wpimath/src/test/python/geometry/test_rotation2d.py

@@ -0,0 +1,82 @@
+import importlib.util
+import math
+
+import pytest
+
+from wpimath.geometry import Rotation2d
+
+
+@pytest.mark.parametrize(
+    "radians,degrees",
+    [
+        (math.pi / 3, 60.0),
+        (math.pi / 4, 45.0),
+    ],
+)
+def test_radians_to_degrees(radians: float, degrees: float):
+    rot = Rotation2d(radians)
+    assert math.isclose(degrees, rot.degrees())
+
+
+@pytest.mark.parametrize(
+    "degrees,radians",
+    [
+        (45.0, math.pi / 4),
+        (30.0, math.pi / 6),
+    ],
+)
+def test_degrees_to_radians(degrees: float, radians: float):
+    rot = Rotation2d.fromDegrees(degrees)
+    assert math.isclose(radians, rot.radians())
+
+
+def test_rotate_by_from_zero() -> None:
+    zero = Rotation2d()
+    rotated = zero + Rotation2d.fromDegrees(90)
+
+    assert math.isclose(math.pi / 2, rotated.radians())
+    assert math.isclose(90.0, rotated.degrees())
+
+
+def test_rotate_by_non_zero() -> None:
+    rot = Rotation2d.fromDegrees(90.0)
+    rot += Rotation2d.fromDegrees(30.0)
+
+    assert math.isclose(120.0, rot.degrees())
+
+
+def test_minus() -> None:
+    rot1 = Rotation2d.fromDegrees(70)
+    rot2 = Rotation2d.fromDegrees(30)
+
+    assert math.isclose(40.0, (rot1 - rot2).degrees())
+
+
+def test_unary_minus() -> None:
+    rot = Rotation2d.fromDegrees(20)
+    assert math.isclose(-20.0, (-rot).degrees())
+
+
+def test_equality() -> None:
+    rot1 = Rotation2d.fromDegrees(43.0)
+    rot2 = Rotation2d.fromDegrees(43.0)
+    assert rot1 == rot2
+
+    rot1 = Rotation2d.fromDegrees(-180.0)
+    rot2 = Rotation2d.fromDegrees(180.0)
+    assert rot1 == rot2
+
+
+def test_inequality() -> None:
+    rot1 = Rotation2d.fromDegrees(43.0)
+    rot2 = Rotation2d.fromDegrees(43.5)
+    assert rot1 != rot2
+
+
+@pytest.mark.skipif(
+    importlib.util.find_spec("numpy") is None, reason="numpy is not available"
+)
+def test_to_matrix() -> None:
+    before = Rotation2d.fromDegrees(20.0)
+    after = Rotation2d.fromMatrix(before.toMatrix())
+    assert before == after

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)

wpimath/src/test/python/test_controller.py

@@ -0,0 +1,5 @@
+import wpimath.controller
+
+
+def test_todo():
+    pass

wpimath/src/test/python/test_estimator.py

@@ -0,0 +1,5 @@
+import wpimath.estimator
+
+
+def test_todo():
+    pass

wpimath/src/test/python/test_interpolation.py

@@ -0,0 +1,50 @@
+import math
+
+from wpimath.geometry import Pose2d, Rotation2d
+from wpimath.interpolation import (
+    TimeInterpolatableFloatBuffer,
+    TimeInterpolatablePose2dBuffer,
+    TimeInterpolatableRotation2dBuffer,
+)
+
+
+def test_float():
+    buffer = TimeInterpolatableFloatBuffer(10)
+
+    buffer.addSample(0, 0)
+    assert buffer.sample(0) == 0
+    buffer.addSample(1, 1)
+    assert buffer.sample(0.5) == 0.5
+    assert buffer.sample(1) == 1
+    buffer.addSample(3, 2)
+    assert buffer.sample(2) == 1.5
+
+    buffer.addSample(10.5, 2)
+    assert buffer.sample(0) == 1
+
+
+def test_rotation2d():
+    buffer = TimeInterpolatableRotation2dBuffer(10)
+
+    buffer.addSample(0, Rotation2d(0))
+    assert buffer.sample(0) == Rotation2d(0)
+    buffer.addSample(1, Rotation2d(1))
+    assert buffer.sample(0.5) == Rotation2d(0.5)
+    assert buffer.sample(1) == Rotation2d(1)
+    buffer.addSample(3, Rotation2d(2))
+    assert buffer.sample(2) == Rotation2d(1.5)
+
+    buffer.addSample(10.5, Rotation2d(2))
+    assert buffer.sample(0) == Rotation2d(1)
+
+
+def test_pose2d():
+    buffer = TimeInterpolatablePose2dBuffer(10)
+    # We expect to be at (1 - 1/sqrt(2), 1/sqrt(2), 45deg) at t=0.5
+    buffer.addSample(0, Pose2d(0, 0, Rotation2d.fromDegrees(90)))
+    buffer.addSample(1, Pose2d(1, 1, Rotation2d(0)))
+    sample = buffer.sample(0.5)
+    assert sample is not None
+    assert math.isclose(sample.X(), 1 - 1 / 2**0.5)
+    assert math.isclose(sample.Y(), 1 / 2**0.5)
+    assert math.isclose(sample.rotation().degrees(), 45)

wpimath/src/test/python/test_optimization_simulated_annealing.py

@@ -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.
+
+import math
+import random
+
+from wpimath.optimization import SimulatedAnnealing
+
+
+def clamp(v, minval, maxval):
+    return max(min(v, maxval), minval)
+
+
+def test_double_function_optimization_heartbeat():
+    def function(x):
+        return -(x + math.sin(x)) * math.exp(-x * x) + 1
+
+    step_size = 10.0
+
+    simulated_annealing = SimulatedAnnealing(
+        2.0, lambda x: clamp(x + (random.random() - 0.5) * step_size, -3, 3), function
+    )
+
+    solution = simulated_annealing.solve(-1.0, 5000)
+
+    assert math.isclose(solution, 0.68, abs_tol=1e-1)
+
+
+def test_double_function_optimization_multimodal():
+    def function(x):
+        return math.sin(x) + math.sin((10.0 / 3.0) * x)
+
+    step_size = 10.0
+
+    simulated_annealing = SimulatedAnnealing(
+        2.0, lambda x: clamp(x + (random.random() - 0.5) * step_size, 0, 7), function
+    )
+
+    solution = simulated_annealing.solve(-1.0, 5000)
+
+    assert math.isclose(solution, 5.146, abs_tol=1e-1)

wpimath/src/test/python/test_spline.py

@@ -0,0 +1,5 @@
+import wpimath.spline
+
+
+def test_todo():
+    pass

wpimath/src/test/python/test_system.py

@@ -0,0 +1,6 @@
+import wpimath.system
+import wpimath.system.plant
+
+
+def test_todo():
+    pass

wpimath/src/test/python/test_trajectory.py

@@ -0,0 +1,145 @@
+import math
+
+from wpimath.geometry import (
+    Ellipse2d,
+    Pose2d,
+    Rectangle2d,
+    Rotation2d,
+    Transform2d,
+    Translation2d,
+)
+from wpimath.spline import CubicHermiteSpline, SplineHelper
+from wpimath.trajectory import (
+    Trajectory,
+    TrajectoryConfig,
+    TrajectoryGenerator,
+    TrajectoryParameterizer,
+)
+from wpimath.trajectory.constraint import (
+    EllipticalRegionConstraint,
+    MaxVelocityConstraint,
+    RectangularRegionConstraint,
+    TrajectoryConstraint,
+)
+
+
+def getTestTrajectory(config: TrajectoryConfig) -> Trajectory:
+    # 2018 cross scale auto waypoints
+    sideStart = Pose2d.fromFeet(1.54, 23.23, Rotation2d.fromDegrees(180))
+    crossScale = Pose2d.fromFeet(23.7, 6.8, Rotation2d.fromDegrees(-160))
+
+    config.setReversed(True)
+
+    vector = [
+        (
+            sideStart + Transform2d(Translation2d.fromFeet(-13, 0), Rotation2d())
+        ).translation(),
+        (
+            sideStart
+            + Transform2d(
+                Translation2d.fromFeet(-19.5, 5.0), Rotation2d.fromDegrees(-90)
+            )
+        ).translation(),
+    ]
+
+    return TrajectoryGenerator.generateTrajectory(sideStart, vector, crossScale, config)
+
+
+#
+# EllipticalRegionConstraint tests
+#
+
+
+def test_elliptical_region_constraint():
+    maxVelocity = 2
+    ellipse = Ellipse2d.fromFeet(Pose2d.fromFeet(5, 2.5, math.radians(180)), 5, 2.5)
+
+    config = TrajectoryConfig.fromFps(13, 13)
+    maxVelConstraint = MaxVelocityConstraint.fromFps(maxVelocity)
+    regionConstraint = EllipticalRegionConstraint(ellipse, maxVelConstraint)
+
+    config.addConstraint(regionConstraint)
+
+    trajectory = getTestTrajectory(config)
+
+    exceededConstraintOutsideRegion = False
+    for point in trajectory.states():
+        if ellipse.contains(point.pose.translation()):
+            assert abs(point.velocity_fps) < maxVelocity + 0.05
+        elif abs(point.velocity_fps) >= maxVelocity + 0.05:
+            exceededConstraintOutsideRegion = True
+
+        # translation = point.pose.translation()
+        # if translation.x_feet < 10 and translation.y_feet < 5:
+        #     assert abs(point.velocity_fps) < maxVelocity + 0.05
+        # elif abs(point.velocity_fps) >= maxVelocity + 0.05:
+        #     exceededConstraintOutsideRegion = True
+
+    assert exceededConstraintOutsideRegion
+
+
+#
+# RectangularRegionConstraint tests
+#
+
+
+def test_rectangular_region_constraint():
+    maxVelocity = 2
+    rectangle = Rectangle2d(Translation2d.fromFeet(1, 1), Translation2d.fromFeet(5, 27))
+
+    config = TrajectoryConfig.fromFps(13, 13)
+    maxVelConstraint = MaxVelocityConstraint.fromFps(maxVelocity)
+    regionConstraint = RectangularRegionConstraint(rectangle, maxVelConstraint)
+
+    config.addConstraint(regionConstraint)
+
+    trajectory = getTestTrajectory(config)
+
+    exceededConstraintOutsideRegion = False
+    for point in trajectory.states():
+        if rectangle.contains(point.pose.translation()):
+            assert abs(point.velocity_fps) < maxVelocity + 0.05
+        elif abs(point.velocity_fps) >= maxVelocity + 0.05:
+            exceededConstraintOutsideRegion = True
+
+    assert exceededConstraintOutsideRegion
+
+
+#
+# TrajectoryConstraint
+#
+
+
+def test_trajectory_constraint_min_max():
+    min_max = TrajectoryConstraint.MinMax()
+    _, _ = min_max
+
+    min_max = TrajectoryConstraint.MinMax(minAcceleration=0, maxAcceleration=1)
+    assert (
+        repr(min_max)
+        == "TrajectoryConstraint.MinMax(minAcceleration=0.0, maxAcceleration=1.0)"
+    )
+
+
+#
+# TrajectoryParameterizer
+#
+
+
+def test_trajectory_parameterizer():
+    start = Pose2d(1, 1, 0)
+    end = Pose2d(2, 2, math.pi / 2)
+
+    # generate the spline from start and end poses
+    vec1, vec2 = SplineHelper.cubicControlVectorsFromWaypoints(start, [], end)
+    spline = CubicHermiteSpline(vec1.x, vec2.x, vec1.y, vec2.y)
+
+    # sample the pose and curvature along the spline
+    points: list[tuple[Pose2d, float]] = []
+    for i in range(100):
+        points.append(spline.getPoint(i / 100))
+
+    trajectory = TrajectoryParameterizer.timeParameterizeTrajectory(
+        points, [], 0, 0, 4, 3, False
+    )
+    assert trajectory is not None

wpimath/src/test/python/test_trajectory_exponential_profile.py

@@ -0,0 +1,319 @@
+# 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.
+
+import pytest
+
+from wpimath.controller import SimpleMotorFeedforwardMeters
+from wpimath.trajectory import ExponentialProfileMeterVolts
+
+kDt = 0.01
+feedforward = SimpleMotorFeedforwardMeters(0, 2.5629, 0.43277, kDt)
+constraints = ExponentialProfileMeterVolts.Constraints.fromCharacteristics(
+    12, 2.5629, 0.43277
+)
+
+
+def assert_near(val1, val2, eps):
+    assert (
+        abs(val1 - val2) <= eps
+    ), f"Difference between {val1} and {val2} is greater than {eps}"
+
+
+def assert_near_state(val1, val2, eps):
+    assert_near(val1.position, val2.position, eps)
+    assert_near(val1.position, val2.position, eps)
+
+
+def check_dynamics(
+    profile: ExponentialProfileMeterVolts,
+    current: ExponentialProfileMeterVolts.State,
+    goal: ExponentialProfileMeterVolts.State,
+):
+    next_state = profile.calculate(kDt, current, goal)
+
+    signal = feedforward.calculate(current.velocity, next_state.velocity)
+
+    assert abs(signal) < constraints.maxInput + 1e-9
+
+    return next_state
+
+
+@pytest.fixture
+def profile():
+    return ExponentialProfileMeterVolts(constraints)
+
+
+def test_reaches_goal(profile):
+    goal = ExponentialProfileMeterVolts.State(10, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    for _ in range(450):
+        state = check_dynamics(profile, state, goal)
+
+    assert state == goal
+
+
+def test_pos_continuous_under_vel_change(profile):
+    goal = ExponentialProfileMeterVolts.State(10, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    for i in range(300):
+        if i == 150:
+            profile = ExponentialProfileMeterVolts(
+                ExponentialProfileMeterVolts.Constraints.fromStateSpace(
+                    9, constraints.A, constraints.B
+                )
+            )
+
+        state = check_dynamics(profile, state, goal)
+
+    assert state == goal
+
+
+def test_pos_continuous_under_vel_change_backward(profile):
+    goal = ExponentialProfileMeterVolts.State(-10, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    for i in range(300):
+        if i == 150:
+            profile = ExponentialProfileMeterVolts(
+                ExponentialProfileMeterVolts.Constraints.fromStateSpace(
+                    9, constraints.A, constraints.B
+                )
+            )
+
+        state = check_dynamics(profile, state, goal)
+
+    assert state == goal
+
+
+def test_backwards(profile):
+    goal = ExponentialProfileMeterVolts.State(-10, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    for _ in range(400):
+        state = check_dynamics(profile, state, goal)
+
+    assert state == goal
+
+
+def test_switch_goal_in_middle(profile):
+    goal = ExponentialProfileMeterVolts.State(-10, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    for _ in range(50):
+        state = check_dynamics(profile, state, goal)
+
+    assert state != goal
+
+    goal = ExponentialProfileMeterVolts.State(0.0, 0.0)
+    for _ in range(100):
+        state = check_dynamics(profile, state, goal)
+
+    assert state == goal
+
+
+def test_top_speed(profile):
+    goal = ExponentialProfileMeterVolts.State(40, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    max_speed = 0
+    for _ in range(900):
+        state = check_dynamics(profile, state, goal)
+        max_speed = max(max_speed, state.velocity)
+
+    assert_near(constraints.maxVelocity(), max_speed, 10e-5)
+    assert state == goal
+
+
+def test_top_speed_backward(profile):
+    goal = ExponentialProfileMeterVolts.State(-40, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    max_speed = 0
+    for _ in range(900):
+        state = check_dynamics(profile, state, goal)
+        max_speed = min(max_speed, state.velocity)
+
+    assert_near(-constraints.maxVelocity(), max_speed, 10e-5)
+    assert state == goal
+
+
+def test_large_initial_velocity(profile):
+    goal = ExponentialProfileMeterVolts.State(40, 0)
+    state = ExponentialProfileMeterVolts.State(0, 8)
+
+    for _ in range(900):
+        state = check_dynamics(profile, state, goal)
+
+    assert state == goal
+
+
+def test_large_negative_initial_velocity(profile):
+    goal = ExponentialProfileMeterVolts.State(-40, 0)
+    state = ExponentialProfileMeterVolts.State(0, -8)
+
+    for _ in range(900):
+        state = check_dynamics(profile, state, goal)
+
+    assert state == goal
+
+
+class ETestCase:
+    def __init__(self, initial, goal, inflection_point):
+        self.initial = initial
+        self.goal = goal
+        self.inflection_point = inflection_point
+
+
+def test_heuristic(profile):
+    test_cases = [
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -4),
+            ExponentialProfileMeterVolts.State(0.75, -4),
+            ExponentialProfileMeterVolts.State(1.3758, 4.4304),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -4),
+            ExponentialProfileMeterVolts.State(1.4103, 4),
+            ExponentialProfileMeterVolts.State(1.3758, 4.4304),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.6603, 4),
+            ExponentialProfileMeterVolts.State(0.75, -4),
+            ExponentialProfileMeterVolts.State(1.3758, 4.4304),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.6603, 4),
+            ExponentialProfileMeterVolts.State(1.4103, 4),
+            ExponentialProfileMeterVolts.State(1.3758, 4.4304),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -4),
+            ExponentialProfileMeterVolts.State(0.5, -2),
+            ExponentialProfileMeterVolts.State(0.4367, 3.7217),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -4),
+            ExponentialProfileMeterVolts.State(0.546, 2),
+            ExponentialProfileMeterVolts.State(0.4367, 3.7217),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.6603, 4),
+            ExponentialProfileMeterVolts.State(0.5, -2),
+            ExponentialProfileMeterVolts.State(0.5560, -2.9616),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.6603, 4),
+            ExponentialProfileMeterVolts.State(0.546, 2),
+            ExponentialProfileMeterVolts.State(0.5560, -2.9616),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -4),
+            ExponentialProfileMeterVolts.State(-0.75, -4),
+            ExponentialProfileMeterVolts.State(-0.7156, -4.4304),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -4),
+            ExponentialProfileMeterVolts.State(-0.0897, 4),
+            ExponentialProfileMeterVolts.State(-0.7156, -4.4304),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.6603, 4),
+            ExponentialProfileMeterVolts.State(-0.75, -4),
+            ExponentialProfileMeterVolts.State(-0.7156, -4.4304),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.6603, 4),
+            ExponentialProfileMeterVolts.State(-0.0897, 4),
+            ExponentialProfileMeterVolts.State(-0.7156, -4.4304),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -4),
+            ExponentialProfileMeterVolts.State(-0.5, -4.5),
+            ExponentialProfileMeterVolts.State(1.095, 4.314),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -4),
+            ExponentialProfileMeterVolts.State(1.0795, 4.5),
+            ExponentialProfileMeterVolts.State(-0.5122, -4.351),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.6603, 4),
+            ExponentialProfileMeterVolts.State(-0.5, -4.5),
+            ExponentialProfileMeterVolts.State(1.095, 4.314),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.6603, 4),
+            ExponentialProfileMeterVolts.State(1.0795, 4.5),
+            ExponentialProfileMeterVolts.State(-0.5122, -4.351),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -8),
+            ExponentialProfileMeterVolts.State(0, 0),
+            ExponentialProfileMeterVolts.State(-0.1384, 3.342),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, -8),
+            ExponentialProfileMeterVolts.State(-1, 0),
+            ExponentialProfileMeterVolts.State(-0.562, -6.792),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, 8),
+            ExponentialProfileMeterVolts.State(1, 0),
+            ExponentialProfileMeterVolts.State(0.562, 6.792),
+        ),
+        ETestCase(
+            ExponentialProfileMeterVolts.State(0.0, 8),
+            ExponentialProfileMeterVolts.State(-1, 0),
+            ExponentialProfileMeterVolts.State(-0.785, -4.346),
+        ),
+    ]
+
+    for test_case in test_cases:
+        state = profile.calculateInflectionPoint(test_case.initial, test_case.goal)
+        assert_near_state(test_case.inflection_point, state, 1e-3)
+
+
+def test_timing_to_current(profile):
+    goal = ExponentialProfileMeterVolts.State(2, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    for _ in range(400):
+        state = check_dynamics(profile, state, goal)
+        assert_near(profile.timeLeftUntil(state, state), 0, 2e-2)
+
+
+def test_timing_to_goal(profile):
+    goal = ExponentialProfileMeterVolts.State(2, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    predicted_time_left = profile.timeLeftUntil(state, goal)
+    reached_goal = False
+    for _ in range(400):
+        state = check_dynamics(profile, state, goal)
+
+        if not reached_goal and state == goal:
+            # Expected value using for loop index is just an approximation since
+            # the time left in the profile doesn't increase linearly at the
+            # endpoints
+            assert_near(predicted_time_left, _ / 100.0, 0.25)
+            reached_goal = True
+
+
+def test_timing_to_negative_goal(profile):
+    goal = ExponentialProfileMeterVolts.State(-2, 0)
+    state = ExponentialProfileMeterVolts.State(0, 0)
+
+    predicted_time_left = profile.timeLeftUntil(state, goal)
+    reached_goal = False
+    for _ in range(400):
+        state = check_dynamics(profile, state, goal)
+
+        if not reached_goal and state == goal:
+            # Expected value using for loop index is just an approximation since
+            # the time left in the profile doesn't increase linearly at the
+            # endpoints
+            assert_near(predicted_time_left, _ / 100.0, 0.25)
+            reached_goal = True

wpimath/src/test/python/test_trapezoid_profile.py

@@ -0,0 +1,24 @@
+import pytest
+
+from wpimath import trajectory
+
+trapezoid_profile_types = [
+    trajectory.TrapezoidProfile,
+    trajectory.TrapezoidProfileRadians,
+]
+
+
+@pytest.mark.parametrize("TrapezoidProfile", trapezoid_profile_types)
+def test_constraints_repr(TrapezoidProfile):
+    expected_qualname = f"{TrapezoidProfile.__name__}.Constraints"
+    constraints = TrapezoidProfile.Constraints()
+
+    assert repr(constraints).startswith(f"{expected_qualname}(maxVelocity=0.")
+
+
+@pytest.mark.parametrize("TrapezoidProfile", trapezoid_profile_types)
+def test_state_repr(TrapezoidProfile):
+    expected_qualname = f"{TrapezoidProfile.__name__}.State"
+    constraints = TrapezoidProfile.State()
+
+    assert repr(constraints).startswith(f"{expected_qualname}(position=0.")

wpimath/src/test/python/test_units.py

@@ -0,0 +1,71 @@
+import pytest
+import wpimath_test
+
+
+def test_units_attributes():
+    assert wpimath_test.SomeClass.s_constant == 2
+    assert wpimath_test.SomeClass.ms_constant1 == 20  # the unit is ms, not seconds
+    assert wpimath_test.SomeClass.ms_constant2 == 0.050
+    assert wpimath_test.SomeClass.ms_constant3 == 200
+
+
+def test_units_check_default_by_name1():
+    sc = wpimath_test.SomeClass()
+
+    assert sc.checkDefaultByName1(0.020) == True
+    assert sc.checkDefaultByName1() == True
+
+    with pytest.raises(RuntimeError):
+        sc.checkDefaultByName1(100)
+
+
+def test_units_check_default_by_name2():
+    sc = wpimath_test.SomeClass()
+
+    assert sc.checkDefaultByName2(0.050) == True
+    assert sc.checkDefaultByName2() == True
+
+    with pytest.raises(RuntimeError):
+        sc.checkDefaultByName2(100)
+
+
+def test_units_check_default_by_num1():
+    sc = wpimath_test.SomeClass()
+
+    assert sc.checkDefaultByNum1(0.050) == True
+    assert sc.checkDefaultByNum1() == True
+
+    with pytest.raises(RuntimeError):
+        sc.checkDefaultByNum1(100)
+
+
+def test_units_check_default_by_num2():
+    sc = wpimath_test.SomeClass()
+
+    assert sc.checkDefaultByNum2(0.050) == True
+    assert sc.checkDefaultByNum2() == True
+
+    with pytest.raises(RuntimeError):
+        sc.checkDefaultByNum2(100)
+
+
+def test_units_ft():
+    assert wpimath_test.SomeClass.five_ft == 5.0
+
+
+def test_units_ft2m():
+    sc = wpimath_test.SomeClass()
+
+    assert sc.ft2m(3) == 0.9144
+
+
+def test_units_ms2s():
+    sc = wpimath_test.SomeClass()
+
+    assert sc.ms2s(20) == 0.020
+
+
+def test_units_s2ms():
+    sc = wpimath_test.SomeClass()
+
+    assert sc.s2ms(0.2) == 200.0

wpimath/src/test/python/trajectory/test_trapezoidal_profile.py

@@ -0,0 +1,176 @@
+# 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.
+
+
+import math
+
+from wpimath.trajectory import TrapezoidProfile
+
+kDt = 0.01  # 10 ms
+
+
+def test_reaches_goal():
+    constraints = TrapezoidProfile.Constraints(1.75, 0.75)
+    goal = TrapezoidProfile.State(3.0, 0.0)
+    state = TrapezoidProfile.State()
+
+    profile = TrapezoidProfile(constraints)
+    for _ in range(450):
+        state = profile.calculate(kDt, state, goal)
+    assert state == goal
+
+
+def test_pos_continuous_under_vel_change():
+    constraints = TrapezoidProfile.Constraints(1.75, 0.75)
+    goal = TrapezoidProfile.State(12.0, 0.0)
+    profile = TrapezoidProfile(constraints)
+    state = profile.calculate(kDt, TrapezoidProfile.State(), goal)
+
+    last_pos = state.position
+    for i in range(1600):
+        if i == 400:
+            constraints = TrapezoidProfile.Constraints(0.75, 0.75)
+            profile = TrapezoidProfile(constraints)
+
+        state = profile.calculate(kDt, state, goal)
+        estimated_vel = (state.position - last_pos) / kDt
+
+        if i >= 400:
+            if estimated_vel <= constraints.maxVelocity:
+                assert estimated_vel <= constraints.maxVelocity
+            else:
+                assert math.isclose(
+                    estimated_vel, constraints.maxVelocity, abs_tol=1e-4
+                )
+            assert state.velocity <= constraints.maxVelocity
+
+        last_pos = state.position
+
+    assert state == goal
+
+
+def test_backwards():
+    constraints = TrapezoidProfile.Constraints(0.75, 0.75)
+    goal = TrapezoidProfile.State(-2.0, 0.0)
+    state = TrapezoidProfile.State()
+    profile = TrapezoidProfile(constraints)
+
+    for _ in range(400):
+        state = profile.calculate(kDt, state, goal)
+    assert state == goal
+
+
+def test_switch_goal_in_middle():
+    constraints = TrapezoidProfile.Constraints(0.75, 0.75)
+    goal = TrapezoidProfile.State(-2.0, 0.0)
+    state = TrapezoidProfile.State()
+    profile = TrapezoidProfile(constraints)
+
+    for _ in range(200):
+        state = profile.calculate(kDt, state, goal)
+    assert state != goal
+
+    goal = TrapezoidProfile.State(0.0, 0.0)
+    profile = TrapezoidProfile(constraints)
+    for _ in range(550):
+        state = profile.calculate(kDt, state, goal)
+    assert state == goal
+
+
+def test_top_speed():
+    constraints = TrapezoidProfile.Constraints(0.75, 0.75)
+    goal = TrapezoidProfile.State(4.0, 0.0)
+    state = TrapezoidProfile.State()
+    profile = TrapezoidProfile(constraints)
+
+    for _ in range(200):
+        state = profile.calculate(kDt, state, goal)
+    assert math.isclose(constraints.maxVelocity, state.velocity, abs_tol=1e-4)
+
+    profile = TrapezoidProfile(constraints)
+    for _ in range(2000):
+        state = profile.calculate(kDt, state, goal)
+    assert state == goal
+
+
+def test_timing_to_current():
+    constraints = TrapezoidProfile.Constraints(0.75, 0.75)
+    goal = TrapezoidProfile.State(2.0, 0.0)
+    state = TrapezoidProfile.State()
+    profile = TrapezoidProfile(constraints)
+
+    for _ in range(400):
+        state = profile.calculate(kDt, state, goal)
+        assert math.isclose(profile.timeLeftUntil(state.position), 0.0, abs_tol=0.02)
+
+
+def test_timing_to_goal():
+    constraints = TrapezoidProfile.Constraints(0.75, 0.75)
+    goal = TrapezoidProfile.State(2.0, 0.0)
+    profile = TrapezoidProfile(constraints)
+
+    state = profile.calculate(kDt, goal, TrapezoidProfile.State())
+    predicted_time_left = profile.timeLeftUntil(goal.position)
+
+    reached_goal = False
+    for i in range(400):
+        state = profile.calculate(kDt, state, goal)
+        if not reached_goal and state == goal:
+            assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.25)
+            reached_goal = True
+
+
+def test_timing_before_goal():
+    constraints = TrapezoidProfile.Constraints(0.75, 0.75)
+    goal = TrapezoidProfile.State(2.0, 0.0)
+    profile = TrapezoidProfile(constraints)
+
+    state = profile.calculate(kDt, goal, TrapezoidProfile.State())
+    predicted_time_left = profile.timeLeftUntil(1.0)
+
+    reached_goal = False
+    for i in range(400):
+        state = profile.calculate(kDt, state, goal)
+        if not reached_goal and abs(state.velocity - 1.0) < 1e-4:
+            assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.02)
+            reached_goal = True
+
+
+def test_timing_to_negative_goal():
+    constraints = TrapezoidProfile.Constraints(0.75, 0.75)
+    goal = TrapezoidProfile.State(-2.0, 0.0)
+    profile = TrapezoidProfile(constraints)
+
+    state = profile.calculate(kDt, goal, TrapezoidProfile.State())
+    predicted_time_left = profile.timeLeftUntil(goal.position)
+
+    reached_goal = False
+    for i in range(400):
+        state = profile.calculate(kDt, state, goal)
+        if not reached_goal and state == goal:
+            assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.25)
+            reached_goal = True
+
+
+def test_timing_before_negative_goal():
+    constraints = TrapezoidProfile.Constraints(0.75, 0.75)
+    goal = TrapezoidProfile.State(-2.0, 0.0)
+    profile = TrapezoidProfile(constraints)
+
+    state = profile.calculate(kDt, goal, TrapezoidProfile.State())
+    predicted_time_left = profile.timeLeftUntil(-1.0)
+
+    reached_goal = False
+    for i in range(400):
+        state = profile.calculate(kDt, state, goal)
+        if not reached_goal and abs(state.velocity + 1.0) < 1e-4:
+            assert math.isclose(predicted_time_left, i * kDt, abs_tol=0.02)
+            reached_goal = True
+
+
+def test_initialization_of_current_state():
+    constraints = TrapezoidProfile.Constraints(1.0, 1.0)
+    profile = TrapezoidProfile(constraints)
+    assert math.isclose(profile.timeLeftUntil(0.0), 0.0, abs_tol=1e-10)
+    assert math.isclose(profile.totalTime(), 0.0, abs_tol=1e-10)