[vesc_servo_controller.cpp] Nonlinear treatment of integral terms at saturation boundaries

[nyxrobotics]

Abstract

In the following program, the behavior of the I-control appears to be non-linear near saturation.

https://github.com/sbgisen/vesc/blob/09f74141f0381c09940c48a6aff31642ac7a2188/vesc_hw_interface/src/vesc_servo_controller.cpp#L93-L101

I think the correct solution is as follows.

    if (isSaturated(u_pid))
    {
      u = saturate(u_pid);
    }