Update wpimath tests, add additional fromFeet to Translation2d

Author: virtuald

subprojects/robotpy-wpimath/gen/geometry/Translation2d.yml

@@ -51,6 +51,9 @@ inline_code: |
     .def_static("fromFeet", [](units::foot_t x, units::foot_t y){
       return std::make_unique<Translation2d>(x, y);
     }, py::arg("x"), py::arg("y"))
+    .def_static("fromFeet", [](units::foot_t distance, const Rotation2d &angle) {
+      return std::make_unique<Translation2d>(distance, angle);
+    }, py::arg("distance"), py::arg("angle"))
     .def_property_readonly("x", &Translation2d::X)
     .def_property_readonly("y", &Translation2d::Y)
     .def_property_readonly("x_feet", [](Translation2d * self) -> units::foot_t {

subprojects/robotpy-wpimath/tests/test_trajectory.py

@@ -4,7 +4,7 @@
 import wpimath.trajectory.constraint
 from wpimath.spline import CubicHermiteSpline, SplineHelper
 
-from wpimath.geometry import Pose2d, Rotation2d, Translation2d
+from wpimath.geometry import Ellipse2d, Pose2d, Rectangle2d, Rotation2d, Translation2d
 
 from wpimath.trajectory import Trajectory, TrajectoryConfig
 
@@ -47,38 +47,30 @@ def getTestTrajectory(config: TrajectoryConfig) -> Trajectory:
 
 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.fromFeet(
-        Translation2d.fromFeet(5, 2.5),
-        10,
-        5,
-        Rotation2d.fromDegrees(180),
-        maxVelConstraint,
-    )
+    regionConstraint = EllipticalRegionConstraint(ellipse, maxVelConstraint)
 
     config.addConstraint(regionConstraint)
 
     trajectory = getTestTrajectory(config)
 
     exceededConstraintOutsideRegion = False
     for point in trajectory.states():
-        translation = point.pose.translation()
-        if translation.x_feet < 10 and translation.y_feet < 5:
+        if ellipse.contains(point.pose.translation()):
             assert abs(point.velocity_fps) < maxVelocity + 0.05
         elif abs(point.velocity_fps) >= maxVelocity + 0.05:
             exceededConstraintOutsideRegion = True
 
-    assert exceededConstraintOutsideRegion
-
-
-def test_elliptical_region_is_pose_in_region():
-    maxVelConstraint = MaxVelocityConstraint.fromFps(2)
-    regionConstraintNoRotation = EllipticalRegionConstraint.fromFeet(
-        Translation2d.fromFeet(1, 1), 2, 4, Rotation2d(0), maxVelConstraint
-    )
+        # 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 not regionConstraintNoRotation.isPoseInRegion(Pose2d.fromFeet(2.1, 1, 0))
+    assert exceededConstraintOutsideRegion
 
 
 #
@@ -88,37 +80,26 @@ def test_elliptical_region_is_pose_in_region():
 
 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(
-        Translation2d.fromFeet(1, 1),
-        Translation2d.fromFeet(5, 27),
-        maxVelConstraint,
-    )
+    regionConstraint = RectangularRegionConstraint(rectangle, maxVelConstraint)
 
     config.addConstraint(regionConstraint)
 
     trajectory = getTestTrajectory(config)
 
     exceededConstraintOutsideRegion = False
     for point in trajectory.states():
-        if regionConstraint.isPoseInRegion(point.pose):
+        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
 
 
-def test_rectangular_region_is_pose_in_region():
-    maxVelConstraint = MaxVelocityConstraint.fromFps(2)
-    regionConstraint = RectangularRegionConstraint(
-        Translation2d.fromFeet(1, 1), Translation2d.fromFeet(5, 27), maxVelConstraint
-    )
-    assert not regionConstraint.isPoseInRegion(Pose2d())
-    assert regionConstraint.isPoseInRegion(Pose2d.fromFeet(3, 14.5, Rotation2d()))
-
-
 #
 # TrajectoryParameterizer
 #

subprojects/robotpy-wpimath/tests/test_trajectory_exponential_profile.py

@@ -8,7 +8,7 @@
 from wpimath.trajectory import ExponentialProfileMeterVolts
 
 kDt = 0.01
-feedforward = SimpleMotorFeedforwardMeters(0, 2.5629, 0.43277)
+feedforward = SimpleMotorFeedforwardMeters(0, 2.5629, 0.43277, kDt)
 constraints = ExponentialProfileMeterVolts.Constraints.fromCharacteristics(
     12, 2.5629, 0.43277
 )
@@ -32,7 +32,7 @@ def check_dynamics(
 ):
     next_state = profile.calculate(kDt, current, goal)
 
-    signal = feedforward.calculate(current.velocity, next_state.velocity, kDt)
+    signal = feedforward.calculate(current.velocity, next_state.velocity)
 
     assert abs(signal) < constraints.maxInput + 1e-9