My team is looking at using Jaci’s Pathfinder this coming season, and I have a question. I understand that there are basically two components to Pathfinder, the generation of a path, and telling your robot to follow that path (I realize that this is grossly oversimplifying the process, but bear with me).
My question is, if an unknown variable causes a change to the state of the robot while it is executing a path (i.e. another robot hits your robot and turns your robot 90 degrees), will Pathfinder notice the discrepancy between the desired robot state and the actual robot state, and make a course correction? Or is the idea behind Pathfinder to minimize the amount of unknown variables (i.e. wheel slippage, jerking) so as to render a more consistent outcome during autonomous without automatically making course corrections?
Path following itself is dependent on your sensors giving feedback and telling you what the current state of the robot is. As long as your sensors can recognize an unexpected change in state, the robot should be able to get back on track, albeit with some error. So, it really comes down to how and to what accuracy can you measure how far your robot has traveled.
In your example, it’s largely dependent on whether or not the robot can detect a change in heading independent of the drivetrain. With encoder-based feedback, you won’t see this because the wheels will slip across the carpet and this isn’t going to be registered. More than likely, the robot will attempt to drive the remainder of the path in the direction it’s headed, instead of the desired direction.
Even if you are using a gyro, Pathfinder will “attempt” to correct back, although in reality, it cannot do this with accuracy because it has no knowledge of the robot state. It simply uses a P controller on heading and PDVA on encoder feedback, but it doesn’t look at the robot state vs. desired state as a whole. If you actually want to go back onto the trajectory, you can still use a Pathfinder generated trajectory; you’ll just need to use a separate controller like Pure Pursuit instead of the Encoder Follower steer back onto the trajectory.
So would I be correct in saying that if a robot is following a path, and is ‘derailed’ from that path via another robot hitting it and turning it, the only way for the robot to reliably get back onto the original path, would be to measure the amount that the robot has rotated (via a gyro), and generate a new path that puts the robot back on track, using the current known state of the robot as a starting point?
Put another way, say for example the robot was unable to generate paths, but could follow paths imported as .csv files. Even if the robot had a gyro and could measure undesired change in the heading of the robot, is it safe to say that if such a robot were rotated by an external force while in the process of following a path, the robot would be unable to make a course correction, and would continue to follow the pre-set path at the wrong heading?
Pathfinder v1 does not actively track robot pose; it tracks robot state through encoder (and, optionally, gyro) readings with EncoderFollower, but this will not actually “correct for” the type of disturbance you describe, so much as it will merely do its best to keep the wheel velocities and heading appropriate for where the robot currently is in the pre-computed path. This can easily be seen from the fact that Pathfinder v1’s implementation is based around pre-computing a trajectory, and then following that trajectory with a separate piece of code that does not know anything about robot pose.
To do the sort of “course-correction” you describe with Pathfinder v1, you would need some way of periodically re-computing your Pathfinder trajectory during movement. Pathfinder v1 is not designed to do this (for example, it implicitly assumes your robot starts from a stationary state), and it would be rather difficult to hack together a solution using it.
I believe Pathfinder v2 is planned to support this functionality.
But for this to work, you need to be aware that you’ll need a way to compute your robot configuration (position + heading) reliably, which is not an easy problem to solve.
To iterate on some of what was said above, PFv1 will attempt, the best it can, to keep you on that predefined trajectory. It tries to match up your wheel encoder counts and gyro heading to those in the path (as well as velocity and acceleration). If you get knocked too far off course, it might be impossible for it to reach the path again since the robot is not physically capable of going fast enough. PFv1 is designed to overcome small disturbances. 90deg may be recoverable from, but your path tracking from that point in will be subpar.
A lot of these problems can be overcome with PFv2, since you can generate trajectories on-the-fly, but these solutions are an order of magnitude more difficult to implement than the normal following from PFv1, from both a library and user code standpoint.
PFv2 will (after initial release), have support for Pure-Pursuit, so that may be the ‘easy way out’ of this problem if you can find a way to reliably track your robot configuration throughout the match.
Is there any timeline on when PathFinder V2 will be released? My team is currently using PathFinder V1, and wondering if we should create our own Pure Pursuit algorithm or wait for PathFinder V2.
Also Prateek, in your Pure Pursuit algorithm, do you use the PathFinder generated trajectories or create your own cubic/quintic splines? Which method would you recommend?
Over the offseason, we have moved over to custom quintic spline generation (so that we could learn more about spline generation and trajectories, etc.). However, for the pure pursuit algorithm itself, there is no point in going the custom route. Pathfinder’s splines and trajectories contain all the information (x, y, theta, velocity, etc.) you need to get pure pursuit up and running. I would recommend Pathfinder because you don’t have to spend the time coming up with a solution to generate splines yourself.
I don’t know anything about PFv2’s release timeline (@jaci) but if you want to get started with pure pursuit or similar algorithms I would read about the Ramsete algorithm. Tyler Veness’s Practical Guide to State Space Control has plenty of info on the Ramsete algorithm, which is sort of an optimized pure pursuit controller for wheeled robots. It’s on page 137.
If you have questions, the best place to find help would probably be the #programming channel on the FRC Discord, there are several people there who have implemented this algorithm and would be happy to help.
On that note, I noticed on the Github page for PFv2 that there only seems to be support for Java (and I think C++). Do you plan on adding support for Labview like in PFv1?
Also, has anyone ever implemented a swerve drive base with Pathfinder? I know there is a swerve modifier in PFv1, but I would love to hear if someone successfully used it, and what the pros/cons were.
“Not an easy problem to solve” may be an understatement. Kalman Filters, non-linear odometry, and advanced control theory is a daunting topic that to many feels out of reach. Will Pathfinder 2 require some form of this to function? Will something basic be built in, or will teams be required to write their own positioning algorithms? I suppose you could always just plug in the estimated state based on the position in the spline, but I don’t know how that would work in an APP controller.
You can do reasonably well by using gyro and encoder odometry under the assumption that the robot’s local movement is always approximately circular. This doesn’t require any filtering unless you want to fuse it with other sensor readings.
Alternatively, you could try using solvePNP with a camera, if you have a vision target, though I have no idea how well this would work during robot movement.
There are many ways to approach solving this problem. It sounds like Jaci is thinking about implementing a form of feedback planning in Pathfinder V2, using the actual (estimated) state of the robot to replan a complete solution to the goal. This is an awesome capability to have, but as noted it requires the ability to estimate state accurately. It also requires very fast replanning (otherwise, by the time your new plan is ready, your robot is no longer where you thought it was!) and the ability to accept arbitrary robot configurations without generating errors or poor quality plans. The latter two requirements can be tricky to implement, especially if you plan under dynamics constraints (limits on the forces/accelerations the robot can exert).
It’s worth noting that replanning of this sort is not the only way to implement a “correcting” robot navigation solution. Controllers like pure pursuit or the aforementioned Ramsete nonlinear time-varying controller can correct for small disturbances off the path as long as you have a good state estimator (which, as noted previously in this thread, can be effective with just some encoders, a gyro, and some simplifying assumptions about how the robot moves in between measurements).
A common approach in robotics is to only replan if the tracking error gets too large (which hopefully never happens), and rely on these local controllers to keep the robot on track in the presence of small errors. In the context of FRC, I think this is a great approach - often, if you are off your intended path by a lot, it may be more prudent to just stop entirely (to avoid damage to the robot, or fouls) - or maybe fall back to a simpler/slower autonomous routine - than to count on replanning your way out of trouble dynamically. After all, the reason you are so far off is likely due to some unforeseen circumstance (such as running into your partner, or robot damage) that your planner doesn’t know how to reason about anyhow!
You can see it takes in a few things, but most notably are an instance of state, some curves and the profile.
The curve lookup is only done based on the distance thru the curve. In this sense, “on the fly” means it can account for small changes (like feeding back in the velocity to avoid a ‘runaway follower’), and that the whole path doesn’t need to be pregenerated (you can do it piece by piece). It also means you can start from a non-zero velocity. You can think of this implementation like that of Pathfinder v1, but without requiring a full pregeneration. Essentially, you can take the ‘feedback’ stage of your control loop and embed it in the trajectory generator itself, ensuring you’re never commanding your robot to go faster than is physically possible. It’s still a path follower, but with some extra bells.
The curves list can be mutated for curves you haven’t traversed yet, so if you detect a sudden change to your robot that throws it off course, you can generate a new (partial) path from your current position to your goal without stopping the control loop (or at the least, pausing it for a very short amount of time). This is what I’m getting at when I say “you can regenerate the path”. This requires that you DO know your robot configuration at any given time, and also relies on YOU knowing when to generate a new path. This is a hard way around the problem, but still qualifies as path following. It has the advantage of working around disturbances while still following a strict path. For something simpler which doesn’t necessarily follow a strict path, Pure Pursuit / Ramsete are the best ways forward.
This thread also lets me talk about something I want to eventually implement in Pathfinder v2, which comes from Chapter 7 of “Robot Motion Planning” (Latombe, 1990)*, which is less of “path following and generation” and more of a field of small nudges. It’s based on potential fields, which requires relative positions of your goal point and any obstacles (notice relative, it doesn’t require absolute positioning). It constructs an attractive force at the goal point, and repulsive forces at each obstacle. Follow the forces, find the goal. It’s more suited to holonomic drivetrains, but can also work with coupled / differential drive systems. It’s also got a few caveats, but can be great for robots that employ a LiDAR system, and have moving obstacles (like other robots, wink wink).
** a lot of PFv2’s nomenclature also comes from this book. It’s a great read if you get a chance*
