Path Following #

Teams almost always use RR to generate and follow trajectories that specify where the robot should be at every point in time. To follow a trajectory, the robot controller starts a stopwatch and directs the feedback controller to close the gap between the estimated pose and the desired pose at the current time from the trajectory. A key advantage of this approach is the clear termination criterion: when the stopwatch hits the trajectory duration.

The main disadvantage is that you can’t make full use of your robot’s capabilities. The feedback controller needs some headroom to speed up if it falls behind, and that headroom leaves performance on the table. And if the robot gets ahead of its trajectory, the feedback controller will push it backwards away from the goal which leaves time on the table.

If you want to push your robot to its limits, you can try a different following paradigm that doesn’t used a fixed temporal schedule. Instead of using a stopwatch to compute a desired pose, the controller finds the closest point on the path to the robot and moves based on it.

This approach is similar to pure pursuit which you may have heard of before. The main difference is that our proposed path follower doesn’t have a lookahead and attempts higher-accuracy tracking of the path instead of merely pushing toward the goal.

To see how to implement this, let’s work up from the HolonomicController class. It implements one method with the signature

fun compute(
    targetPose: Pose2dDual<Time>,
    actualPose: Pose2d,
    actualVelActual: PoseVelocity2d,
): PoseVelocity2dDual<Time>

The latter two arguments come from the localizer as with trajectory. The question is how to get targetPose, a pose on the path and its derivative with respect to time. We can call project(path, actualPose, displacementGuess) to get a displacement along path. (project() works iteratively, and you can help it by passing the last displacement as a guess for the next loop iteration.) Now the target pose is given by path.get(displacement, 3) where the 3 indicates that derivatives up to second order are desired. But these derivatives aren’t quite what compute() wants. They are with respect to the displacement instead of time (i.e., we have a Pose2dDual<Arclength> instead of a Pose2dDual<Time>). Following the types, there’s a method on Pose2dDual<T> that does what we want:

fun <NewParam> reparam(oldParam: DualNum<NewParam>) =
    Pose2dDual(position.reparam(oldParam), heading.reparam(oldParam))

Now we just need the displacement/arclength as a DualNum<Time>. One way to compute that is evaluating a DisplacementProfile at the displacement value from project(). It’s not quite kinematically correct, though it works when actualPose is close to targetPose (which should hopefully be the case). (A more correct expression is left as an exercise to the reader. Hint: try taking the derivative of the orthogonality constraint between the path point/derivatives and the query point.)

Returning to the practical side, there are additional changes to truly push the robot velocity is fast as possible. The velocity command calculation in HolonomicController assumes that adding the feedforward command and feedback command together will always be feasible (i.e., within physical voltage limits). To guarantee this while maximizing the commanded speed of the motors, start by computing the feedback

val error = targetPose.value().minusExp(actualPose)

val feedbackCommand = PoseVelocity2d(
    Vector2d(
        axialPosGain * error.position.x,
        lateralPosGain * error.position.y,
    ),
    headingGain * error.heading.log(),
) +
PoseVelocity2d(
    Vector2d(
        axialVelGain * velErrorActual.linearVel.x,
        lateralVelGain * velErrorActual.linearVel.y,
    ),
    headingVelGain * velErrorActual.angVel,
)

Then take the raw Pose2dDual<Arclength> from before and compute the maximum path displacement velocity/time derivative that satisfies the constraints (the commanded voltage for any motor is no larger than the current battery voltage). You probably don’t care about predicting acceleration in this formulation and should operate on the assumption that kA is zero.