From f8ac8027303861cb993fa7e519f4ee1746273a75 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Sat, 28 Sep 2024 14:44:52 -0400 Subject: [PATCH] Generate path point at exact locations of rotation targets --- lib/path/pathplanner_path.dart | 31 ++++++++++++++----- pathplannerlib-python/pathplannerlib/path.py | 25 ++++++++++----- pathplannerlib/.vscode/settings.json | 3 +- .../pathplanner/lib/path/PathPlannerPath.java | 28 ++++++++++++----- .../pathplanner/lib/path/PathPlannerPath.cpp | 30 +++++++++++++----- 5 files changed, 88 insertions(+), 29 deletions(-) diff --git a/lib/path/pathplanner_path.dart b/lib/path/pathplanner_path.dart index 49cebd96..4d61e4f8 100644 --- a/lib/path/pathplanner_path.dart +++ b/lib/path/pathplanner_path.dart @@ -408,18 +408,35 @@ class PathPlannerPath { } } - // Add a rotation target to the previous point if it is closer to it than - // the current point - if (unaddedTargets.isNotEmpty) { - if ((unaddedTargets[0].waypointRelativePos - prevPos).abs() <= - (unaddedTargets[0].waypointRelativePos - pos).abs()) { - pathPoints.last.rotationTarget = unaddedTargets.removeAt(0); + // Add rotation targets + RotationTarget? target; + PathPoint prevPoint = pathPoints.last; + + while (unaddedTargets.isNotEmpty && + unaddedTargets[0].waypointRelativePos >= prevPos && + unaddedTargets[0].waypointRelativePos <= pos) { + if ((unaddedTargets[0].waypointRelativePos - prevPos).abs() < 0.001) { + // Close enough to prev pos + prevPoint.rotationTarget = unaddedTargets.removeAt(0); + } else if ((unaddedTargets[0].waypointRelativePos - pos).abs() < + 0.001) { + // Close enough to next pos + target = unaddedTargets.removeAt(0); + } else { + // We should insert a point at the exact position + RotationTarget t = unaddedTargets.removeAt(0); + pathPoints.add(PathPoint( + position: samplePath(t.waypointRelativePos), + rotationTarget: t, + constraints: _constraintsForPos(t.waypointRelativePos), + waypointPos: t.waypointRelativePos, + )); } } pathPoints.add(PathPoint( position: position, - rotationTarget: null, + rotationTarget: target, constraints: _constraintsForPos(pos), waypointPos: pos, )); diff --git a/pathplannerlib-python/pathplannerlib/path.py b/pathplannerlib-python/pathplannerlib/path.py index 935b19b0..ca9386dc 100644 --- a/pathplannerlib-python/pathplannerlib/path.py +++ b/pathplannerlib-python/pathplannerlib/path.py @@ -773,14 +773,25 @@ def _createPath(self) -> List[PathPoint]: pos = pos + (correctIncrement * 0.5) position = self._samplePath(pos) - # Add a rotation target to the previous point if it is closer to it than - # the current point - if len(unaddedTargets) > 0: - if abs(unaddedTargets[0].waypointRelativePosition - prevWaypointPos) <= abs( - unaddedTargets[0].waypointRelativePosition - pos): - points[-1].rotationTarget = unaddedTargets.pop(0) + # Add rotation targets + target: Union[RotationTarget, None] = None + prevPoint = points[-1] + + while len(unaddedTargets) > 0 and prevWaypointPos <= unaddedTargets[0].waypointRelativePosition <= pos: + if abs(unaddedTargets[0].waypointRelativePosition - prevWaypointPos) < 0.001: + # Close enough to prev pos + prevPoint.rotationTarget = unaddedTargets.pop(0) + elif abs(unaddedTargets[0].waypointRelativePosition - pos) < 0.001: + # Close enough to next pos + target = unaddedTargets.pop(0) + else: + # We should insert a point at the exact position + t = unaddedTargets.pop(0) + points.append(PathPoint(self._samplePath(t.waypointRelativePosition), t, + self._constraintsForWaypointPos(t.waypointRelativePosition))) + points[-1].waypointRelativePos = t.waypointRelativePosition - points.append(PathPoint(position, None, self._constraintsForWaypointPos(pos))) + points.append(PathPoint(position, target, self._constraintsForWaypointPos(pos))) points[-1].waypointRelativePos = pos pos = min(pos + targetIncrement, numSegments) diff --git a/pathplannerlib/.vscode/settings.json b/pathplannerlib/.vscode/settings.json index 7d060d05..57b620b4 100644 --- a/pathplannerlib/.vscode/settings.json +++ b/pathplannerlib/.vscode/settings.json @@ -100,6 +100,7 @@ "xlocmes": "cpp", "xlocmon": "cpp", "xlocnum": "cpp", - "xloctime": "cpp" + "xloctime": "cpp", + "xtree": "cpp" } } \ No newline at end of file diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index 094baff8..afb61e8d 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -614,16 +614,30 @@ private List createPath() { } } - // Add a rotation target to the previous point if it is closer to it than - // the current point - if (!unaddedTargets.isEmpty()) { - if (Math.abs(unaddedTargets.get(0).getPosition() - prevWaypointPos) - <= Math.abs(unaddedTargets.get(0).getPosition() - pos)) { - points.get(points.size() - 1).rotationTarget = unaddedTargets.remove(0); + // Add rotation targets + RotationTarget target = null; + PathPoint prevPoint = points.get(points.size() - 1); + + while (!unaddedTargets.isEmpty() + && unaddedTargets.get(0).getPosition() >= prevWaypointPos + && unaddedTargets.get(0).getPosition() <= pos) { + if (Math.abs(unaddedTargets.get(0).getPosition() - prevWaypointPos) < 0.001) { + // Close enough to prev pos + prevPoint.rotationTarget = unaddedTargets.remove(0); + } else if (Math.abs(unaddedTargets.get(0).getPosition() - pos) < 0.001) { + // Close enough to next pos + target = unaddedTargets.remove(0); + } else { + // We should insert a point at the exact position + RotationTarget t = unaddedTargets.remove(0); + points.add( + new PathPoint( + samplePath(t.getPosition()), t, constraintsForWaypointPos(t.getPosition()))); + points.get(points.size() - 1).waypointRelativePos = t.getPosition(); } } - points.add(new PathPoint(position, null, constraintsForWaypointPos(pos))); + points.add(new PathPoint(position, target, constraintsForWaypointPos(pos))); points.get(points.size() - 1).waypointRelativePos = pos; pos = Math.min(pos + targetIncrement, numSegments); } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp index e3062f10..a20d4855 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp @@ -381,18 +381,34 @@ std::vector PathPlannerPath::createPath() { } } - // Add a rotation target to the previous point if it is closer to it than - // the current point - if (!unaddedTargets.empty()) { + // Add rotation targets + std::optional < RotationTarget > target = std::nullopt; + PathPoint prevPoint = points[points.size() - 1]; + + while (!unaddedTargets.empty() + && unaddedTargets[0].getPosition() >= prevWaypointPos + && unaddedTargets[0].getPosition() <= pos) { if (std::abs(unaddedTargets[0].getPosition() - prevWaypointPos) - <= std::abs(unaddedTargets[0].getPosition() - pos)) { - points[points.size() - 1].rotationTarget = unaddedTargets[0]; + < 0.001) { + // Close enough to prev pos + prevPoint.rotationTarget = unaddedTargets[0]; unaddedTargets.erase(unaddedTargets.begin()); + } else if (std::abs(unaddedTargets[0].getPosition() - pos) + < 0.001) { + // Close enough to next pos + target = unaddedTargets[0]; + unaddedTargets.erase(unaddedTargets.begin()); + } else { + // We should insert a point at the exact position + RotationTarget t = unaddedTargets[0]; + unaddedTargets.erase(unaddedTargets.begin()); + points.emplace_back(samplePath(t.getPosition()), t, + constraintsForWaypointPos(t.getPosition())); + points[points.size() - 1].waypointRelativePos = t.getPosition(); } } - points.emplace_back(position, std::nullopt, - constraintsForWaypointPos(pos)); + points.emplace_back(position, target, constraintsForWaypointPos(pos)); points[points.size() - 1].waypointRelativePos = pos; pos = std::min(pos + targetIncrement, static_cast(numSegments)); }