Have you tried adding the `desired_state.velocity` as a feed-forward component to the command?. In the current implementation the PID sees both the reference command and the tracking error as perturbations to compensate for, which will probably require larger gains. If OTOH you add a feed-forward `desired_state.velocity` (just add the term to the above line), the perturbations the PID has to compensate become much smaller, hence will require smaller gains.