From e70096858c078395a2ba5556f22b17a743bec739 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 11 Dec 2023 18:53:01 -0500 Subject: [PATCH] improve code for timing rotations --- .../robot/serialization/TimedRotation.java | 8 ++ .../command/SendableCommand.java | 4 + .../gui/pathing/TrajectoryPathRenderer.java | 80 +++++++++++++------ .../autobuilder/gui/util/MathUtil.java | 16 ++++ 4 files changed, 85 insertions(+), 23 deletions(-) diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/TimedRotation.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/TimedRotation.java index 04d0c08..b70366e 100644 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/TimedRotation.java +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/TimedRotation.java @@ -37,4 +37,12 @@ public TimedRotation(@NotNull TimedRotation other) { public Rotation2d getRotation() { return rotation; } + + @Override + public String toString() { + return "TimedRotation{" + + "time=" + time + + ", rotation=" + rotation + + '}'; + } } diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableCommand.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableCommand.java index cd49fc9..41a2a89 100644 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableCommand.java +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableCommand.java @@ -42,8 +42,11 @@ public class SendableCommand { @JsonProperty("command") private final boolean command; + @JsonIgnoreProperties private boolean shouldWait; + @JsonIgnoreProperties + private boolean shouldCancelCommand; private static final @NotNull Map> INFERABLE_TYPES_PARSER; @@ -69,6 +72,7 @@ public class SendableCommand { INFERABLE_TYPES_PARSER.put(Boolean.class.getName(), Boolean::valueOf); } + @JsonIgnoreProperties private static final List primitiveTypes = Arrays.asList( int.class.getName(), double.class.getName(), diff --git a/core/src/main/com/dacubeking/autobuilder/gui/pathing/TrajectoryPathRenderer.java b/core/src/main/com/dacubeking/autobuilder/gui/pathing/TrajectoryPathRenderer.java index fb7dba1..01632c1 100644 --- a/core/src/main/com/dacubeking/autobuilder/gui/pathing/TrajectoryPathRenderer.java +++ b/core/src/main/com/dacubeking/autobuilder/gui/pathing/TrajectoryPathRenderer.java @@ -802,41 +802,75 @@ public Trajectory getTrajectory() { return rotation2dList; } + + record CloseTimedRotations(double distance, TimedRotation timedRotation) implements Comparable { + @Override + public int compareTo(@NotNull TrajectoryPathRenderer.CloseTimedRotations o) { + if (Math.abs(Double.compare(this.distance, o.distance)) < 0.01) { + // If we're really close do the one that is earlier in the path + return Double.compare(this.timedRotation.time, o.timedRotation.time); + } + return Double.compare(this.distance, o.distance); + } + } + + /** * @return List of rotation points and the times they should be used * @throws NullPointerException if the trajectory has not been generated yet. Call {@link #getTrajectory()} to get the * trajectory. */ public List getRotationsAndTimes() { - List rotationsAndTimes = new ArrayList<>(rotation2dList.size()); - int currentIndexPos = 0; - rotationsAndTimes.add(new TimedRotation(0.0, rotation2dList.get(0))); - for (double i = 0; i < Objects.requireNonNull(trajectory).getTotalTimeSeconds(); i += 0.01f) { - Vector3 renderVector = MathUtil.toRenderVector3(trajectory.sample(i).poseMeters); - - if (currentIndexPos + 1 < controlVectors.size() && - MathUtil.toRenderVector3(controlVectors.get(currentIndexPos + 1).x[0], - controlVectors.get(currentIndexPos + 1).y[0]) - .dst2(renderVector) < 8f) { - currentIndexPos++; - rotationsAndTimes.add(new TimedRotation(i, rotation2dList.get(currentIndexPos))); - } + List> closeRotationsList = new ArrayList<>(controlVectors.size()); + for (int i = 0; i < controlVectors.size(); i++) { + closeRotationsList.add(new ArrayList<>()); } + for (int i = 0; i < Objects.requireNonNull(trajectory).getStates().size() - 1; i++) { + State state = trajectory.getStates().get(i); + State stateNext = trajectory.getStates().get(i + 1); + Vector2 point = MathUtil.toVector2(state.poseMeters); + Vector2 pointNext = MathUtil.toVector2(stateNext.poseMeters); + + var maxDistance2 = MathUtil.dist2(point, pointNext); + + + for (int j = 0; j < controlVectors.size(); j++) { + var targetPoint = MathUtil.toVector2( + controlVectors.get(j).x[0], + controlVectors.get(j).y[0] + ); + + Vector2 closestPoint = MathUtil.getClosestPointOnSegment(point, pointNext, targetPoint); - if (rotation2dList.size() - rotationsAndTimes.size() > 1) { - NotificationHandler.addNotification(new Notification( - Color.ORANGE, - "Warning: Path has " + (rotation2dList.size() - rotationsAndTimes.size()) + " extra rotations at the end of" + - " the path", - 2000)); + double time = state.timeSeconds; + if (!(closestPoint.epsilonEquals(point, 0.01f) || closestPoint.epsilonEquals(pointNext, 0.01f))) { + Vector2 distanceBetweenStates = pointNext.cpy().sub(point); + float distanceToClosestPoint = point.dst(closestPoint); + float lenBetweenStates = distanceBetweenStates.len(); + double interpolationAmount = distanceToClosestPoint / lenBetweenStates; + + time = state.timeSeconds + interpolationAmount * (stateNext.timeSeconds - state.timeSeconds); + } else if (closestPoint.epsilonEquals(pointNext, 0.01f)) { + time = stateNext.timeSeconds; + } + + float distanceToPoint = closestPoint.dst2(targetPoint); + if (distanceToPoint < maxDistance2) { + closeRotationsList.get(j).add(new CloseTimedRotations(distanceToPoint, new TimedRotation(time, + rotation2dList.get(j)))); + } + } } - for (int i = currentIndexPos + 1; i < rotation2dList.size(); i++) { - rotationsAndTimes.add(new TimedRotation(trajectory.getTotalTimeSeconds(), rotation2dList.get(i))); + var timedRotations = new ArrayList(controlVectors.size()); + + for (List closeRotations : closeRotationsList) { + timedRotations.add(closeRotations.stream().sorted().findFirst() + .orElse(new CloseTimedRotations(0, new TimedRotation())).timedRotation()); } - assert rotationsAndTimes.size() == rotation2dList.size(); - return rotationsAndTimes; + assert timedRotations.size() == rotation2dList.size(); + return timedRotations; } public void removeSelection() { diff --git a/core/src/main/com/dacubeking/autobuilder/gui/util/MathUtil.java b/core/src/main/com/dacubeking/autobuilder/gui/util/MathUtil.java index 506628d..1dbe19c 100644 --- a/core/src/main/com/dacubeking/autobuilder/gui/util/MathUtil.java +++ b/core/src/main/com/dacubeking/autobuilder/gui/util/MathUtil.java @@ -9,6 +9,10 @@ import org.jetbrains.annotations.NotNull; public class MathUtil { + + private MathUtil() { + } + public static float clamp(float val, float min, float max) { return Math.max(min, Math.min(max, val)); } @@ -26,6 +30,18 @@ public static float len2(@NotNull Vector2 vec, float x, float y) { return ((x - vec.x) * (x - vec.x)) + ((y - vec.y) * (y - vec.y)); } + + @Contract("_ -> new") + public static @NotNull Vector2 toVector2(@NotNull Pose2d poseMeters) { + return new Vector2((float) poseMeters.getTranslation().getX(), + (float) poseMeters.getTranslation().getY()); + } + + @Contract("_,_ -> new") + public static @NotNull Vector2 toVector2(double x, double y) { + return new Vector2((float) x, (float) y); + } + @Contract("_ -> new") public static @NotNull Vector2 toRenderVector2(@NotNull Pose2d poseMeters) { float pointScaleFactor = AutoBuilder.getConfig().getPointScaleFactor();