I was doing some research on the different Motion Profile path follower implementations, and noted a discrepancy between Pathfinder and TrajectoryLib’s PID calculation for the kD term.
double calculated_value =
kP * error + // Proportional
kD * ((error - last_error) / seg.dt) + // Derivative
(kV * seg.velocity + kA * seg.acceleration); // V and A Terms
I apologize for any misinterpretation, my controls knowledge isn’t the best.
TrajectoryLib’s follower subtracts the current path segment’s target velocity from the robot velocity, then multiplies that by kD
while Pathfinder simply multiplies the robot velocity by kD.
My interpretation of this is trajectoryLib’s version means kD is only acting on the robot’s velocity error from the segment target velocity, rather than just the robot’s velocity. This makes more sense then operating on the robot velocity, so I’m wondering what was the reasoning behind this discrepancy.
Your interpretation of the two implementations is backwards; TrajectoryLib acts only on the derivative of the process variable, as it subtracts off the setpoint derivative, while Pathfinder acts on the derivative of the error. To see this, consider the following:
If we subtract off d(setpoint)/dt (as is done in the TrajectoryLib implementation), we are left only with -d(PV)/dt - the derivative of the process variable. I am not quite sure why it is implemented this way instead of simply calculating the derivative of the process variable directly, however, as it seems needlessly more-prone to roundoff error.
The Pathfinder implementation is a better implementation, so long as the setpoint is changing “continuously” (i.e., is being updated every loop) and smoothly (i.e., no large jumps in setpoint). Under these conditions, the setpoint derivative portion of the term is usable and acts to hold the loop output at the desired rate of change. Removing it causes the derivative term to simply resist all change, desired or not. This can still be useful for damping oscillatory behavior, but is suboptimal; the full derivative term can be extremely useful in responding quickly to small disturbances.
However, if the setpoint is being updated sparsely or discontinuously, the Pathfinder implementation will result in the D term yielding, essentially, a delta-function output every few loop iterations when the setpoint jumps value, while behaving identically to the TrajectoryLib implementation at all other times. This sometimes leads to jerky/undesirable loop behavior, and it can be better to simply remove the setpoint derivative term.
A good example of this is the Talon SRX’s Motion Profiling mode, which currently uses the TrajectoryLib implementation, because, as the control loop runs at 1kHz yet the motion profile points are likely to update around only 50Hz, the setpoint will only actually update about once every 20 control loop iterations.
A better option than removing the setpoint derivative term if the setpoints are sparse, in my opinion, is to either interpolate setpoints, or else filter the setpoint velocity term to smooth and “distribute” it evenly.
Note (like Oblarg has mentioned above), that kD acts on the error derivative.
In the Pathfinder implementation, the kD term acts directly on the change in error over time. In TrajectoryLib, the segment velocity is additionally subtracted from this. With ‘setpoint’ meaning the segments target, and ‘process variable’ meaning the measured state of the system, we get the equation found above (expressed with D(x) = d(x)/dt):
D(error) = D(sp - pv) = D(sp) - D(pv)
With segment velocity being equal to the derivative of the setpoint position, we get segment velocity = D(sp).
Looking at just the derivative term, we get the following:
Pathfinder: kD * (D(sp) - D(pv))
TrajectoryLib: kD * (D(sp) - D(pv) - D(sp)) = kD * (-D(pv))
Remember ‘pv’ is the process variable, in this case, the measured position of our robot, meaning D(pv) is the velocity of our robot (change in position)
In Pathfinder, we apply kD on the error between our desired velocity and our current velocity. As we approach our desired velocity (D(sp)), if our measured velocity (D(pv)) is too high, it will actively push the derivative term into the negatives in order to reduce the applied power. As we get closer to our velocity target, the D term becomes negligible (near 0). Likewise, if our measured velocity is too low, it will increase throttle to increase our measured velocity to meet the target.
This Pathfinder implementation does rely on the fact that the setpoint changes are constant and not too sudden, as mentioned by Eli. To understand why, consider the case where the setpoint changes from 0 to 1 instantly. D(sp) = d(sp)/dt is going to be huge, since dt is close to 0 (in this case, D(sp) is approximately infinite). This is known as a ‘kick’, and sends the derivative term to infinity for that instant. With a Pathfinder path, this is a non problem since points are generated at consistent intervals with no sudden jumps, but this may prove troublesome with custom generated (or invalid) paths.
Now consider the TrajectoryLib implementation, simply kD * -D(pv). The setpoint is not present in here, just the process variable, providing a dampening effect on the system as the velocity (change in measured position over time, D(pv)) gets higher. Since no setpoint is considered, this can be problematic for systems with a constantly (and consistently) changing setpoint, since the dampening effect is the same in all cases. It does, however, work better for systems with suddenly changing (or inconsistently changing) setpoints, since there is no more “D(sp) is roughly infinity” issue.
It should be noted that “suddenly changing” in this respect is the speed of setpoint change compared to the speed of the control loop. (In Pathfinder, the setpoint changes at the same rate the control loop runs, so using the derivative of the error instead of just the process variable makes sense)
This explanation makes a ton of sense to me. Yet after doing some more research, and reading the linked controlguru article, I came across this quote from 971’s Austin Schuh that seems to explain the trajectoryLib controller a bit more.
My favorite controller for following trajectories is as follows (I think this is a modification of 3). I used it before I got tired of hand-tuning the PD for the drivetrain and switched to state feedback.
pwm = P * error + D * ((error - prev_error) / dt - goal_velocity) + Kv * goal_velocity + Ka * goal_acceleration
This is close to a PD controller, but with some feed forward terms. The idea behind feed forwards is that if you have a pretty good idea about how much power it will take to do something, go ahead and add it in. The control loop will take up the slop from there when you are wrong, or someone bumped the bot.
The D term is special in that you subtract off the goal velocity. This falls out from the state feedback controllers. Think about it this way. If you are at the goal, and moving at the right speed, you don’t want to apply corrective power to decelerate the robot (This is what D would do if you weren’t trying to move but were moving and at the goal.)
His explanation makes a decent amount of sense as well. The assertion that the derivative of error equals the derivative of the process variable (with opposite sign) only hold’s true when the setpoint is constant, which is not the case for a Motion Profile follower.
The question I now have is, what is the difference in practice between these two controllers, say if one ran an identical motion profile on the same robot with the only difference being subtracting the setpoint velocity from the D term on one run, vs doing nothing on the other?
If you read the above posts, you’ll find that they are consistent with what you’ve stated here; what you’re missing is that the contribution to D from the setpoint derivative is useful, so long as you are not in a situation in which it will cause “kick” due to discontinuities.
I’m not sure what Schuh is trying to say, there. If you are moving at the right speed, then the error derivative without subtracting off the setpoint velocity should be 0, because the process variable derivative is equal to the setpoint derivative. If you subtract off the setpoint derivative, then you end up doing exactly what he says you want to avoid; you are left with the bare process variable derivative, which is constantly resisting any and all motion regardless of whether it is appropriate or not given the behavior of the setpoint.
The question I now have is, what is the difference in practice between these two controllers, say if one ran an identical motion profile on the same robot with the only difference being subtracting the setpoint velocity from the D term on one run, vs doing nothing on the other?
Given a system that doesn’t suffer from setpoint discontinuities, the controller with the full error derivative will almost certainly do a better job at tracking the profile than the one with only the process variable derivative.