Merge JointSaturationLimiter and RateLimiter

[christophfroehlich]

We have a JointSaturationLimiter that limits positions, velocities, and acceleration for the arms. Can we reuse that logic also here, by extending the limiter with a new on_enforce method to also accept the Twist (or TwistStamped) messages and do the limiting there. I suppose we can reuse the logic, and then we hopefully don't need to have two implementations. AFAIK, we can even create a new class for it as Joint in the name implies to be used on joint only.

Originally posted by @destogl in #1346

[christophfroehlich]

@tpoignonec as you have contributed to the RateLimiter, have you ever considered and analyzed the JointSaturationLimiter? Maybe we have a smarter approach there, and could move it to control_toolbox package.

[tpoignonec]

No I never checked out JointSaturationLimiter, but I don't mind taking a look on Monday, it does seem like the two functionalities have much in commun

[tpoignonec]

@christophfroehlich The algorithm in JointSaturationLimiter seems much more robust, especially the mechanism to check predicted position limits for the next sampling time as well as the stopping feasibility w.r.t. max (de)acceleration.
I don't see any reason to not merge either, keeping the logic from JointSaturationLimiter, but the syntax and interface from RateLimiter.

Actually, would it be sufficient to simply extend the RateLimiter functionalities and to use them with the current API ?
(As opposed to directly providing on_enforce-like methods in the control toolbox)
The cost of wrapping the the RateLimiter for joints or twist is pretty limited, but I don't think there is a unique "good" way to do that, for instance for parameters management.
Also, a component could require all rate-limited variables (e.g., joints or Cartesian coordinates) to stay in sync or something, so that if the motion is halted on one joint, it is so for all joints.

[christophfroehlich]

I think we would have to extract the algorithm (like we did from the SpeedLimiter for the RateLimiter), and use it for joints in the joint_limits package, and for generic variables in the RateLimiter here. It would be good to understand if the other algorithm has better behavior as what we have discussed in #240
then it would be worth it