mjansen4857 / pathplanner

A simple yet powerful path planning tool for FRC robots
https://pathplanner.dev
MIT License
397 stars 125 forks source link

Paths get replanned if they have non-zero velocity even if the heading is correct #500

Closed Maddox-RVS closed 11 months ago

Maddox-RVS commented 11 months ago

Pathfinding Transition and Rotation Target Optimizations

1. Default the pathfinding transition between two auto paths in the auto builder/editor to pathfind to the next path's starting pose. Create an option within the path block in the auto builder/editor that when selected causes the drivetrain to pathfind to the closest point of that specific path as it does now.

Further explanation:

The current pathfinding implementation between two paths in the auto builder/editor is not intuitive. As of this moment PathPlanner links two paths together from the first paths ending pose to the closest point of the second path as seen below:

PathPlanner Path Transition Example 1

PathPlanner Path Transition Example 2

While this feature of being able to pathfind to the next paths closest point is cool and can be useful in certain scenarios, this should not be the default.

Sometimes this can cause the user to fight more with the GUI than is needed to achieve a desired outcome, such as when creating and connecting to a path that crosses over itself or contains a loop, causing the drivetrain to effectively skip over a path that the user wants it to follow.

PathPlanner Path Transition Example 3

The intuitive expectation is that the drivetrain will pathfind from the first path's ending pose to the second path's starting pose.

This is why I would suggest that the more intuitive expectation be the default method used when connecting two paths in the auto builder/editor and that an option in the "follow path" block be added that when selected changes the behavior so that the drivetrain pathfinds to the second path's closest point as it does now.

2. There is no easy way to specify the drivetrain's rotational direction (counterclockwise or clockwise) when rotating to a rotation target in the path editor.

Further explanation:

Yes, you can get the desired outcome of either a counterclockwise or clockwise rotation when the drivetrain is rotating to its rotation target.

However the current way is not very intuitive and can add extra unwanted clutter to the GUI elements being displayed on the path. This can also make it harder and more finicky to edit and make small/large changes to a path since this way forces the managing of two rotation targets to achieve the desired direction of rotation.

PathPlanner Path Rotation Target Example 1

PathPlanner Path Rotation Target Example 2

This is why I would suggest an option within the rotation target that allows the user to specify counterclockwise or clockwise rotation when the drivetrain attempts to rotate to that rotation target's specifications.

This makes forcing a rotational direction easily manageable in one rotation target instead of two, and makes the path editing experience more intuitive and easy to grasp.

mjansen4857 commented 11 months ago

What you've described is path replanning, not pathfinding. Replanning is meant to ease the transition between disjoint paths (within reason), allow for starting pre-planned paths from an unknown position (i.e. some scoring path in teleop), and react to collisions in auto causing large errors. It is not something to be relied on to create autos. If you want an auto to transition from one path's end to the start every time, then align their start/end points. There is a linked waypoint feature in the next release that makes this easier. If you try to string together a bunch of random paths that you wouldn't actually run in auto, replanning can do weird things. But this is not what it is meant for. If you wouldn't string together paths like that without replanning, then don't do it if replanning is enabled. So, this is not going to be changed to support a use-case that just generally shouldn't be done in the first place.

Forcing a rotation direction can be done, but would need quite a bit of generation changes to support it, so it will probably be low priority for a while, at least until python support is finished. I opened a separate issue for it: #501.

Maddox-RVS commented 11 months ago

Thanks for reading my issue. However, I am still having a problem with the replanning feature in the sense that transitioning between two paths when the second path is a loop causes the loop path to be effectively skipped because of the replanning feature.

Skipping Swoop Path

The ending pose of the first path is exactly the same as the starting pose for the next path, except it seems the replanning that should be acting upon disjointed paths seems to still be kicking into action despite the paths not being disjointed. There are no discontinuities as seen below in the first paths ending and the second paths starting pose.

Place 9 Flat Ending Pose

Swoop 4 Starting Pose

mjansen4857 commented 11 months ago

Yeah that's not supposed to happen. I'll take a look