My team is looking to implement real-time path planning to line up with the vision targets this year. The values used for positioning come from the solve3D values from a limelight camera. Any ideas I know of creating paths in advanced using different applications but nothing in real time.

Watch this video from 254 on motion profiling, they outline how they create paths for their autonomous modes. https://www.youtube.com/watch?v=8319J1BEHwM Iâ€™m not sure how fast this would run on a RoboRio, but Iâ€™d give it a shot. If it turns out that the Rio runs too slow, another on-board processor like a Jetson or a raspberry pi might be able to speed things up. Iâ€™m not sure what language you code in but there are plenty of examples of path planning code on github, and if you work in python Iâ€™ve written some code Iâ€™d be happy to share as an example.

Real-time is hard. Standard trajectory generation with things like Pathfinder is really slow - as in 10+ seconds to generate a path. PathfinderV2 is fast enough to generate them in real time, but PFV2 doesnâ€™t quite work yet. 5190â€™s FalconLib has some great fast path planning stuff (based off of the tech that PFv2 and 254 uses to accelerate the generation), but itâ€™s written in Kotlin and harder to interface with from java (though you can *absolutely* do so).

Things like PF/PFv2/FL/CheesyLib are all trajectory generators - they plot a path described by polynomial splines then generates a speed profile to follow the path, with the speed profile generation being the slow aspect (as you have to do numerical integrals, which is hard). PFv2/FL/CL approximates the path using arcs (the length of which is extremely easy to compute), which is why theyâ€™re faster.

Path *followers* work as well - those are things that donâ€™t generate or follow some predetermined trajectory, but instead attempt to steer the robot along a path. Examples include Pure Pursuit and Guiding Vector Fields. PP is pretty damn not worth using these days, if you ask me, but GVF is a great technique that has served me well.

How do you find the function for the level set? It seems like all the â€śintuitiveâ€ť path planners common in FRC output a parametric curve. Is there a way to go from parametric curve -> level curve of a function?

Good question! So, the level set is simply used because you can easily define a vector field direction on either side of the path, and 3D functions provide an easy way to define error. However, I found that you can just project your robotâ€™s pose onto the path (effectively finding the closest point on the path to the robot) and pass that distance through some error function as an alternative to the level set approach â€” that makes GVF 100% compatible with things like polynomial and arc-based splines. As for the normal/tangent/hessian, I just treated the normal and tangent vectors (and by extension, hessian) at the projected point as those at the current point. Worked pretty well!

One issue to note is the fact that projecting onto polynomial splines is really, *really* slow - you have to use a function optimizer like Newtonâ€™s method or simulated annealing. Itâ€™s fast enough to run in real time on a laptop, but I donâ€™t think itâ€™s the sort of thing you could comfortably run at 100Hz on a Rio. Iâ€™m sure with some optimization you could get it to work though.

However, nothing says we need to use polynomial splines! Arcs are very very easy to project onto, so we can either define our paths using biarcs, or you can do what 254/5190/Pathfinder v2 does and approximate a polynomial path using a bunch of arcs â€” they do it to make computing the path length easier, but we can use it here because of the whole its-super-easy-to-project-onto-arcs thing.

I made a test implementation here. Python spaghetti code proof of concept is in the poc/ folder, and half finished Kotlin implementation is under robot/.

One thing to note is that a lot of times you can get large discontinuities in the gradient like so:

However, realistically thatâ€™s never really an issue when following the path. Do notice the undefined behavior before/after the path though â€” thatâ€™ ssomething to watch out for.

Hereâ€™s an example of a simulated robot (orange) following a path (blue):

Out of curiosity, why do you say this? 254 and others have used it to great effect, and as far as I know still are using it.

Iâ€™m fairly sure Solomonâ€™s argument will be that better path followers are more accessible. Last year, 254 used the â€śRamseteâ€ť control law. A good intro to this is in Tylerâ€™s book section 12.4.1.

Just gonna preface by saying that Iâ€™m a scout trying to get more knowledge about programming to do better pit scouting. Whatâ€™s the fastest trajectory generator, and how long does it take?

This is probably a really stupid question.

Anything using arc approximation can generate a path in a trivial amount of time (about 10 ms at most I think.) Non-arc strategies usually take 100+ ms.

Because calculating the length of an arc is very fast, this strategy divides hard-to-measure polynomials into arcs, then calculates the lengths of the arcs to get the path length. The alternative, integration of a square root, is very slow.

IMO this isnâ€™t really a good pit scouting question- it actually doesnâ€™t tell you much about the capability of the team and is a much more micro-level, almost implementation detail.

Now if youâ€™re asking about how fast it is to take waypoints and turn them into a path (for a new autonomous routine, for example) that will depend very much on how fast someone can type them into the computer.

What exactly do you mean by an arc approximation?

Yeah, this is false.

You wonâ€™t find a method with a better tradeoff of â€śworks pretty wellâ€ť to â€śintuitive to understand, teach, and implementâ€ť than pure pursuit.

The algorithm is this:

Take a section of a polynomial (or section of a piecewise function of polynomials aka a spline)

Take the beginning, end, and center points.

Use these three unique points to create an arc.

If this arc is short enough and the difference in curvature between the start and end is small enough, that is your length.

Otherwise, subdivide the section and create two new arcs. Repeat until the entire spline is closely approximated by these arcs.

Sum their lengths, and now you have closely approximated the length of your spline.

You wonâ€™t find a method with a better tradeoff of â€śworks pretty wellâ€ť to â€śintuitive to understand, teach, and implementâ€ť than pure pursuit.

Oh, absolutely, but if youâ€™re putting in the effort to roll your own pathing system, my opinion is that you might as well implement and use something a bit more performant - Iâ€™m super not a fan of the whole â€śintrinsic error on curvesâ€ť thing. YMMV though, itâ€™s true itâ€™ll work well enough.

Just gonna preface by saying that Iâ€™m a scout trying to get more knowledge about programming to do better pit scouting. Whatâ€™s the fastest trajectory generator, and how long does it take?

So much is up to how everythingâ€™s implemented that if you ask me it doesnâ€™t make much sense to use it as a metric; you could generate a trapezoidal profile off of a biarc spline in a matter of milliseconds, but an s-curve generated over a polynomial spline without arc approximation can take tens of seconds. Thereâ€™s pros and cons to every technique, though s-curves over polynomial splines with arc approximation is more or less the state of the art in terms of FRC trajectory generation right now (not counting whatever 900â€™s doing this year, haha).

If youâ€™re just looking for a fast trajectory generator, then Iâ€™d recommend just about anything using arc approx â€” Pathfinderv2 (when itâ€™s finished), 254â€™s library, 5190â€™s library, etc.

Nickâ€™s right in that itâ€™s probably not a great scouting question - Iâ€™d liken it to asking the mechanical team if they used rivets or bolts.

will trajectory generation work without input from the limelight? Iâ€™m sorry if this is a dumb question, Iâ€™m new to this.

As long as you have points to generate from, then yes.

what points would be needed though?

Donâ€™t worry about asking questions!

Itâ€™s totally viable to just have a list of points for different auto routines - for instance last year, we had a set of points to generate a profile from left side of the field to the left scale, right side of the field to the right scale, left switch to right scale, etc.

Though, if you want to get really fancy, you can use something like SolvePnP (I think limelight has a similar function?) that gives your robotâ€™s position and rotation relative to some vision target â€” you can use that to generate paths to the target, for some really accurate pathing and game piece placement. However, you do need to be near the target, so itâ€™s worth considering using preplanned paths to get near, then switching to paths generated from vision data.

5987 has won a Creativity Award and Autonomous Award so far this year for our continuously-regenerating path planning based on 3D cameras using neural network game piece recognition. You can read a bit about it in the robot book I posted in our reveal thread. The vision recognition and path generation is done externally on a Jetson TX2, and the path following is done internally in the roboRIO. Most of it goes way over my head, but Iâ€™m sure the programmers would be happy to answer questions about it if you want to reach out to them.