pid::updateCommand inconsistency with dt=0 or errors

[dcconner]

Line 342 of pid.cpp

If an error or dt=0, occurs you return zero but don't update the stored command.
I saw this causing issues in simulation

I suggest the following:

  if (dt == ros::Duration(0.0))
  {
      return cmd_;
  }
  else if (std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
  {
    i_term_ = 0.0;
    cmd_=0.0;
    return 0.0;
  }

I haven't pushed change to our fork yet, so just putting this up for discussion.

[adolfo-rt]

The docs state that the method should return the PID command, which is clearly not always the case. This means that the current behavior is broken, and that the fix changes the current (broken) behavior.

I wonder if there's people relying on the existing behavior. @jbohren, I remember you worked on this class to fix the error sign convention. Do you have something to say about the current and proposed behaviors?.

@dcconner, I agree that if zero time has elapsed since the last control update, the command should remain the same. When NaNs or infs are encountered, why is the integrator reset (or why not other values as well)?.

If we decide to make a change, the following places should also be updated accordingly:

https://github.com/ros-controls/control_toolbox/blob/indigo-devel/src/pid.cpp#L275
https://github.com/ros-controls/control_toolbox/blob/indigo-devel/src/pid.cpp#L308
https://github.com/ros-controls/control_toolbox/blob/indigo-devel/src/pid.cpp#L371

[dcconner]

p_term and d_term are only internals.
For error storage p_error_ and d_error_ ---> garbage in garbage out (IMHO)

If we reset command, the i_term_ should be reset as well IMHO.

So then the question becomes, do we reset command or hold constant? I think reasonable arguments could be made either way. I suggested reset because I hated the silent failure.