Motion Profiling / Pathfinding Guide

There are dozens of resources about motion profiling and Jaci’s pathfinder, but I’m not too sure what motion profiling is and why it is preferred over PID loops. We’re just getting into complex autonomous this year (laying framework during offseason), so I would like to know the pros and cons of Motion Profiling vs PID. Also, what motion profile framework do you use?

I have read this presentation and am looking for some more white papers and tips.

In addition, does motion profiling work on Mecanum Drives (either in Mecanum mode or Tank mode)? If so, why or why not? Finally, do you have any whitepapers/presentations worth a read/watch?

Sorry for the large number of questions, but our team is trying out complex auto and stuck at the large number of choices.

254 did two awesome presentations at champs in 2015 and 2016, that’s where my team started.

Motion Profiling and Control (2015)

Integrating Computer Vision with Motion Control (2016)

A PID loop is great for getting from point A to point B. However, it does not control the following:

  • How quickly you get there, in seconds
  • Your speed during the maneuver

A PID loop is also a pretty one-dimensional control method. If you want your robot to, for example, drive on a curve, you can’t really achieve the results that you want with a PID loop.

When you introduce motion profiling, you start to control your motion throughout a maneuver. For example, if I want to travel 1 meter in 1 second, then, for example, at time t=0.001 seconds, I might want to be at 0.001 meters, traveling at [some velocity] in meters/second. At t=0.002 seconds, I want to be at 0.003 meters, traveling at [some velocity] in meters/second, etc.

The idea of motion profiling is to control where your robot is throughout a motion, which is different from a PID loop which really only controls where you end up.

If you have a desired position/velocity every 1/1000th of a second, you need a robust way of calculating these points. This is where a pathfinder comes in. 254’s TrajectoryLib or Jaci’s pathfinder are both great options, and I have successfully used the latter (not tried the former). The purpose of a pathfinder is to take your desired waypoints and to fill the time between them with thousands of trajectory points. For example, if I wanted the robot to drive forward 1 meter, to the right 1 meter, and be facing east, (assuming starting at 0, 0, north), I would simply use the following waypoint:

Pathfinder.addWaypoint(1, 1, degrees.90)

Once you have calculated these thousands of waypoints, you just feed them into your motor controllers and let the motor controller execute them. The hardest part of motion profiling is calculating quality waypoints - it is typically easier to execute the waypoints.

In the presentation that you linked above, slide 12 shows the essence of motion profiling. The motion profile shown is a trapezoidal motion profile, because the graph of velocity vs. time is a trapezoid, with three zones: The speed up period (in which the slope of the velocity vs. time graph is your maximum acceleration, because of differential calculus), the flat top of the trapezoid is your cruise velocity, the slow down period (in which the slope of the velocity vs. time graph is your maximum acceleration *-1, because of differential calculus). Here’s the key: By integral calculus, the area under the velocity vs time curve is the distance that you want to travel, and the velocity vs time curve is something that is within the physical capabilities of your robot.

This presentation by 254 certainly does a better job of explaining the theory and practice of motion profiling than me. However, the overall concept can be easy to grasp: By controlling where your robot should be throughout a motion, you can perform that motion more consistently and reliably, especially when given time restraints, like the autonomous period.

It’s really hard to get reliable feedback from mecanum wheels, because the wheels themselves have rollers on them which can cause them to slip. Therefore, it’s hard to execute a motion profile with feedback when using mecanum wheels. However, it is still possible to run the feedforward part of a motion profile with mecanum wheels, which can have decent results. I would advise against mecanum wheels for motion profiled drivetrain maneuvers, but it’s still possible to have some level of success.

What’s the difference between trapezoidal and S-curve profiles?

Trapezoidal profiles are C2 continuous in position (smooth position, continuous velocity, discontinuous acceleration). This assumes you can change acceleration (torque) instantaneously, which is (to within reasonable approximation) a good assumption for DC motors in FRC applications.

So-called S-curves are C3 continuous in position (smooth position, smooth velocity, continuous acceleration, discontinuous jerk). This rate-limits your change in acceleration (torque), which is nice for long-term motor service life and the comfort of human occupants, but is likely unnecessary for most FRC applications. Very top heavy robots might be an exception.

Given the interest in motion profiling and path following the last couple years, I think it’s time for me to make some updated materials. I don’t think Championship conference presentations are the best vehicle for this, but maybe a whitepaper and/or video would be useful.

Won’t be until the offseason, of course.

1 Like

I’d second a video or a couple of them for this. Would be neato!

Yes, please.

Everyone else has seemed to cover everything else, so I’ll answer the mecanum question. You can, but with some work.

If you put it in tank mode, you can use it just like a tank drive with little issues, as long as your encoders are reading accurately. If you want to use it in mecanum mode, you’ll have to do some manual labour to get the code side working (you can look into Pathfinder’s SwerveModifier to see the general idea of what you need to do).

In Pathfinder v2, I’m working on including mecanum drive as one of the default systems to address this shortcoming.

Yes please! This would be FANTASTIC!

A slight correction, but something I think is important:

One should not think of motion profiling as being something in opposition to PID control. It makes little sense to compare “motion profile versus PID.” Motion profiling is not a control loop. Motion profiling is totally orthogonal to closed-loop control.

Closed-loop control is a method to better-achieve a desired value (“setpoint”) of some process variable.

Motion profiling is a method to generate time-domain functions (“profiles”) with desirable properties to be used as setpoints.

The two address entirely different problems; consequently, one can use PID without motion profiling, and one can use motion profiling without PID.

Moreover, when people on Chief Delphi use the term “motion profiling,” it is often implicitly understood that a PID controller (or equivalent) is being used to follow the profile (as this is by far the most-common FRC implementation), so talking about why “motion profiling is preferred over PID loops” is somewhat nonsensical/confusing. Please avoid doing this.

I think it’s worth noting that the use of PID position control (and, to a much lesser extent, velocity control) without some form of motion profiling (note that even a simple setpoint ramp is a form of motion profiling!) is something of an abuse, in that it almost always involves telling a mechanism to do something that it physically cannot do. It is an abuse that we can get away with in some situations in FRC, but it is, I think, misleading to think of “PID” as somehow referring to this by default.

Please stop perpetuating this. It is a myth. A tank-style motion profile (i.e. one that does not involve sideways movement) can be followed just as accurately (or even moreso, due to the lack of wheel scrub) by a mecanum drive as by a tank drive.

Thanks for this post, I understand a lot better now. A few new questions arise though:

When following a trapezoidal velocity curve profile to drive the robot in a straight line, for example, don’t waypoints have velocity and position? If so, does it matter which the PID is trying to control? Or can it do both somehow?

Whether your loop is “closed on” velocity or position when following a profile is up to the user; there is no “correct” choice. The Talon SRX motion profiling mode uses the velocity setpoint only for feedforward, and closes the loop on position, for example.

The “can it do both” question is actually rather interesting, because in a sense, PID naturally does “both” - but only if implemented a certain way.

Consider a PID loop closed on position. If the derivative term in this PID loop is calculated based on error derivative rather than setpoint derivative, then in essence it does the same thing as the proportional gain of a *velocity *loop. However, derivative gain is not always implemented this way (for more details regarding this, see this thread). However, note that the “velocity” here would be calculated from the difference in position setpoints, and is not specified separately as a “velocity setpoint.”

Likewise, the integral gain of a *velocity *loop behaves analogously to the proportional gain of a *position *loop - at least, up to roundoff error and other differences that can be introduced by filtering of the velocity signal.

(If you understand the fundamental theorem of calculus, it should be clear why this is the case).

A key component of Motion Profiling, in our sense, is that the control loop has more knowledge of the system it’s controlling than a typical PID loop, hence you end up with something that looks like PDVA (where VA are feedforward terms). Since motors don’t function on position control, but rather the position is a consequence of the velocity and acceleration, the VA terms make the loop take both of those into account, allowing them to have a say in how the motor is commanded.

In reality, you can use just PID control, but by having some knowledge of your system and carrying those through to the control loop, your system will be a lot more stable and perform better

It’s a stretch to call it an “abuse”.

If all you care about is settling within a certain period of time with less than a given steady state error, PID control is fine on its own. Heck, bang-bang control is often fine on its own. It is often the case that you don’t really care about the intermediate behavior of the system; just get to where you are going quickly and be done with it. Many mechanical systems have dynamics that perform great with well-tuned P, I, and D control terms and discontinuous setpoints.

Motion profiling is valuable when you care about controlling motion throughout the move. This might be because you want to limit the velocities or forces involved. It might be because you want to coordinate the motion of different mechanisms (like sides of a drivetrain). Or it might be because you want to arrive at the steady state faster than you can attain without setpoint generation. (Motion profiling enables the latter because you are bounding the rate of error growth and potentially supplying a feedforward control derived from a model of your plant, both of which let you crank up your feedback gains without sacrificing stability).

Even (kinematic) motion profiles aren’t perfect. There are almost always higher order dynamics at play that are often ignored by your setpoint generator and “absorbed” by feedback control. The key is minimizing the “scale” of these effects relative to the requirements of your control loop, and ensuring you never saturate your actuators.

As it is commonly done in FRC, I feel comfortable calling it an abuse; we get away with a lot because the problem space allows us not to care about the service life of the components. It leads to a lot of people not realizing that setpoint generation is even a thing to be thought about (and instead focusing explicitly on loop tuning), because the “common knowledge” presents the PID loop alone as the whole solution to the control problem, rather than just to a part of it (that of following the setpoint).

I think it’s more fruitful to not think of “PID with motion profiles” and “PID without motion profiles” as fundamentally different things; rather, you always have (implicitly, at least) a “motion profile” of some sort, it’s just that in the case of discontinuous setpoints it’s not a profile with properties that allow for well-controlled motion during transients. After all, a step function is still a function.

Open invite to present at our Capital City Classic Fall Workshop Series this October! We’ll handle all the recording and uploading of the presentation for you… :cool:

Can you expand on this a little? How can mecanum wheels not have wheel scrub that tank drive wheels would have? Does the structure of the wheel somehow compensate for wheel scrub?

On a side note, what would be the point of running a mecanum-driven robot in tank-drive mode for motion control purposes? If you’re running in tank-mode, wouldn’t you be better off just using tank drive?

Because mecanum wheels have rollers.

Does the structure of the wheel somehow compensate for wheel scrub?

Tank drive with standard wheels must scrub the carpet while turning. That’s why it’s called “skid steer”.

On a side note, what would be the point of running a mecanum-driven robot in tank-drive mode for motion control purposes?

Because the mec wheels don’t scrub, and so might actually follow curved paths more accurately (jury is out on this, not enough data yet).

Also because tank-drive control is less complex mathematically/programmatically than using the full capability of mec for path following.

Wouldnt there still be some level of motion not directly in line with the wheels, which would cause some wheel scrub?

The debate over mecanum wheels seems strongly anecdotal… aside from 1986 last year, I really never see teams with mecanum wheels performing motion profiled drive maneuvers “more accurately” than tank drive. On the other hand, well controlled, motion profiled drive maneuvers with tank drive are increasingly common, with dozens of teams (over 100?) being able to perform such movements this year.