Skip to content

Commit 8b70363

Browse files
authored
Make TrajectoryConstraint.MinMax namedtuple-like (#126)
1 parent 4313900 commit 8b70363

File tree

2 files changed

+58
-14
lines changed

2 files changed

+58
-14
lines changed

subprojects/robotpy-wpimath/gen/controls/TrajectoryConstraint.yml

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,3 +15,26 @@ classes:
1515
attributes:
1616
minAcceleration:
1717
maxAcceleration:
18+
inline_code: |
19+
.def(py::init([](
20+
units::meters_per_second_squared_t minAcceleration,
21+
units::meters_per_second_squared_t maxAcceleration) {
22+
return frc::TrajectoryConstraint::MinMax{minAcceleration, maxAcceleration};
23+
}), py::arg("minAcceleration"), py::arg("maxAcceleration"))
24+
25+
.def("__len__", [](const frc::TrajectoryConstraint::MinMax& self) { return 2; })
26+
.def("__getitem__", [](const frc::TrajectoryConstraint::MinMax& self, int index) {
27+
switch (index) {
28+
case 0:
29+
return self.minAcceleration;
30+
case 1:
31+
return self.maxAcceleration;
32+
default:
33+
throw std::out_of_range("TrajectoryConstraint.MinMax index out of range");
34+
}
35+
})
36+
37+
.def("__repr__", [](const frc::TrajectoryConstraint::MinMax &self) {
38+
return py::str("TrajectoryConstraint.MinMax(minAcceleration={}, maxAcceleration={})").format(
39+
self.minAcceleration, self.maxAcceleration);
40+
})

subprojects/robotpy-wpimath/tests/test_trajectory.py

Lines changed: 35 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,22 +1,27 @@
1-
from wpimath.trajectory import TrajectoryGenerator, TrajectoryParameterizer
2-
from wpimath.geometry import Pose2d, Transform2d
3-
import wpimath.trajectory
4-
import wpimath.trajectory.constraint
5-
from wpimath.spline import CubicHermiteSpline, SplineHelper
6-
7-
from wpimath.geometry import Ellipse2d, Pose2d, Rectangle2d, Rotation2d, Translation2d
8-
9-
from wpimath.trajectory import Trajectory, TrajectoryConfig
1+
import math
102

3+
from wpimath.geometry import (
4+
Ellipse2d,
5+
Pose2d,
6+
Rectangle2d,
7+
Rotation2d,
8+
Transform2d,
9+
Translation2d,
10+
)
11+
from wpimath.spline import CubicHermiteSpline, SplineHelper
12+
from wpimath.trajectory import (
13+
Trajectory,
14+
TrajectoryConfig,
15+
TrajectoryGenerator,
16+
TrajectoryParameterizer,
17+
)
1118
from wpimath.trajectory.constraint import (
12-
MaxVelocityConstraint,
1319
EllipticalRegionConstraint,
20+
MaxVelocityConstraint,
1421
RectangularRegionConstraint,
22+
TrajectoryConstraint,
1523
)
1624

17-
import math
18-
import typing
19-
2025

2126
def getTestTrajectory(config: TrajectoryConfig) -> Trajectory:
2227
# 2018 cross scale auto waypoints
@@ -100,6 +105,22 @@ def test_rectangular_region_constraint():
100105
assert exceededConstraintOutsideRegion
101106

102107

108+
#
109+
# TrajectoryConstraint
110+
#
111+
112+
113+
def test_trajectory_constraint_min_max():
114+
min_max = TrajectoryConstraint.MinMax()
115+
_, _ = min_max
116+
117+
min_max = TrajectoryConstraint.MinMax(minAcceleration=0, maxAcceleration=1)
118+
assert (
119+
repr(min_max)
120+
== "TrajectoryConstraint.MinMax(minAcceleration=0.0, maxAcceleration=1.0)"
121+
)
122+
123+
103124
#
104125
# TrajectoryParameterizer
105126
#
@@ -114,7 +135,7 @@ def test_trajectory_parameterizer():
114135
spline = CubicHermiteSpline(vec1.x, vec2.x, vec1.y, vec2.y)
115136

116137
# sample the pose and curvature along the spline
117-
points: typing.Tuple[Pose2d, float] = []
138+
points: list[tuple[Pose2d, float]] = []
118139
for i in range(100):
119140
points.append(spline.getPoint(i / 100))
120141

0 commit comments

Comments
 (0)