Real time path planning

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. 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.

1 Like

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.

1 Like

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.

1 Like

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

what points would be needed though?

Pathweaver is a way that many teams generate their points.

1 Like

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.