mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
40fb9ff562
Resync with `mostrobotpy` This mostly involves the big "ignore almost everything in the HAL project" and some fixups for the Addressable LED classes. Required two small hand fixes to get it building over here with bazel, and with more compiler warnings on. I also manually zeroed out the `repo_url` field in the toml files to avoid unnecessary churn whenever it goes from a release build to a development build. I already did this with `version` field in there, and will do a follow up PR that updates the copybara script to do it automatically. --------- Co-authored-by: Default email <default@default.com>
143 lines
3.9 KiB
Python
143 lines
3.9 KiB
Python
import math
|
|
|
|
from wpimath import (
|
|
CubicHermiteSpline,
|
|
Ellipse2d,
|
|
EllipticalRegionConstraint,
|
|
MaxVelocityConstraint,
|
|
Pose2d,
|
|
Rectangle2d,
|
|
RectangularRegionConstraint,
|
|
Rotation2d,
|
|
SplineHelper,
|
|
TrajectoryConstraint,
|
|
Trajectory,
|
|
TrajectoryConfig,
|
|
TrajectoryGenerator,
|
|
TrajectoryParameterizer,
|
|
Transform2d,
|
|
Translation2d,
|
|
)
|
|
|
|
|
|
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
|