Trajectory Tutorial uses command.andThen() in a way that doesn't always work

[chauser]

In the Trajectory Tutorial https://docs.wpilib.org/en/stable/docs/software/examples-tutorials/trajectory-tutorial/creating-following-trajectory.html, the technique used at line 142

return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));

to ensure that the robot stops at the end of the trajectory does not work in the case that the command is canceled. I discovered this when I tried disabling my (simulated, thankfully) robot while it was following a trajectory -- it just kept on with the motor settings as they were at the time of disabling. The problem is that the code in the andThen is chained to the original command as another command in a SequentialCommandGroup. When the first command is cancelled so is the group, so the andThen is never run. This is really missing functionality the Commands framework -- we need something analogous to finally that is always run, regardless of how the command ends. For my robot I implemented a little helper class that solves the immediate problem, but it would be better if Command objects had a andFinally method taking a Runnable (not a Command) so that all Commands could benefit from this without another layer of wrapping. Here's the code:

package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.Command;
import java.lang.Runnable;

public class CommandsWithFinally extends SequentialCommandGroup {
    Runnable m_finally;
    public CommandsWithFinally(Command... command) {
        super(command);
    }
    public CommandsWithFinally andFinally(Runnable cleanup) {
        m_finally = cleanup;
        return this;
    }
    public void end(boolean interrupted) {
        super.end(interrupted);
        if (m_finally != null) m_finally.run();
    }
}

and here's how I used it in a factory method for building Ramsete commands for different trajectoryies:

return new CommandsWithFinally(
   ramseteCommand.beforeStarting(
      () -> m_drivetrain.resetOdometry(trajectory.getInitialPose()))).andFinally(
         () -> m_drivetrain.tankDriveVolts(0, 0));

The robot now stops when disabled :-).

[chauser]

When I submitted this I debated with myself about making it a feature request for allwpilib vs a bug report for frc-docs; thanks for bringing it into the allwpilib space for discussion. However, I think it also needs to exist as a bug report for frc-docs because a team implementing according to the tutorial will produce something with unexpected, somewhat dangerous, and rather inexplicable, at first sight, behavior. Do you think I should re-report it on frc-docs?

[Starlight220]

No need to re-report it, if the WPI team needs it also in frc-docs they'll open an issue there pointing to this one.

The first time I read this issue, I saw your point - it is unsafe and everything. However, I now re-read it and I tripped on this:

I discovered this when I tried disabling my (simulated, thankfully) robot while it was following a trajectory -- it just kept on with the motor settings as they were at the time of disabling.

On a real robot, all outputs are disabled when the robot is disabled. This isn't viewable from code and isn't shown in simulation (by default -there is an option to show this in sim).

Team code should set motor values to 0 on disable, regardless of anything. If this isn't documented, it should be.

Adding a .finalizedBy() method is risky as overridden commands wouldn't necessarily call super, making the finalizations not run.

Specifically in the case of RamseteCommand as it is a possible safety issue, perhaps we can make it send 0 to the output lambda in case of interruption?

[chauser]

I hadn't realized (or remembered) that disable turns off all outputs (maybe it's all motor outputs? -- I seem to recall the robot being able to do some things in disabled state.)  Setting motor values to 0 on entering disabled state is certainly good practice. The command-based template on which the trajectory-folowing example is based doesn't do so. In fact that was the first change I made upon discovering the problem. It doesn't fully implement the behavior I expected because there are other ways to interrupt a command.

The point you make about failing to call super in the end method of a subclass is a good one and applies equally to use of end itself. For implementing final behavior and thinking only about Java here, would making finalize be a final method of CommandBase and then calling it from the command scheduler after calling end be sufficient? finalizeBy would attach its Runnable argument to the Command object and finalize would call it, if it existed of course.

I think the design of the RamseteCommand intentionally did not set outputs to 0 with the thought being that you might want to end with the robot still moving in anticipation of something else to follow. I haven't investigated in detail but I think that a following command's initialize and execute methods are called in the same cycle as a finish'ed command's end method. The following command would therefore overwrite any 0 setting before it could do mischief, so maybe always setting to 0 in the end method is OK. You should certainly seek opinions from people more intimately familiar with wpilib design choices than I am on this :-).

[Starlight220]

About adding another lifecycle method to the core Command interface - it just won't happen. It's a major addition to command-based and won't happen, especially not in the middle of a season. It can't be named finalize() anyway because of the protected Object.finalize() method. The templates cannot set motors to 0 on disable because there it depends on the user code. It can be documented, however. I am aware of the design decision to not set 0s at the end of the RamseteCommand. This is the reason I suggest that it be set to 0 only in case of interruption. Orthogonally, perhaps a andThenStop() decorator can be added specifically to RamseteCommand with an optional stopping Runnable that defaults to sending 0 over the RamseteCommand's output lambda. This decorator would mutate the current command object instead of constructing a group, and the fact that RamseteCommand isn't used polymorphically makes the C++ implementation simpler (I assume).

Option 1 is a breaking behavioral change, so perhaps it shouldn't be done mid-season. Option 2 isn't breaking and can theoretically be done now.

[chauser]

I don't know that any changes are urgent. A general way to finalize a command sequence that was less kludgy than my workaround would be a nice feature. It doesn't seem urgent given that 1) a workaround exists 2) in most cases setting motor outputs to 0 when entering disabled state is sufficient. But I do think that the documentation for the trajectory example should point out that the behavior of the code currently in the example might not be what you expect in the case of an interrupted command or command sequence.

[prateekma]

We recently merged a change that would set the velocities/voltages to zero in RamseteCommand when it is interrupted.