Improve interpolation and filtering of incoming command streams

[PeterBowman]

Some time ago, I introduced the CommandBuffer class to act as a single-element buffer for high-frequency commands, mostly devised for CSP mode (IPositionDirect):

yarp-devices/libraries/YarpPlugins/TechnosoftIpos/CommandBuffer.hpp

Lines 11 to 30 in 42b3091

	/**
	* @ingroup TechnosoftIpos
	* @brief A buffer for periodic commands implementing linear interpolation.
	*/
	class CommandBuffer
	{
	public:
	void accept(double command);
	double interpolate();
	double getStoredCommand(double * timestamp = nullptr) const;
	void reset(double initialCommand);
	private:
	double storedCommand {0.0};
	double interpolationResult {0.0};
	double commandPeriod {0.0};
	double commandTimestamp {0.0};
	double interpolationTimestamp {0.0};
	mutable std::mutex mutex;
	};

(implementation)

This feature allowed to decouple the rate of incoming commands (those flowing through the YARP network) from the CSP rate (i.e. commands sent via CAN to the iPOS drives) while also performing linear interpolation (LERP). Prior to that, clients needed to send IPositionDirect commands at exactly the same rate as --syncPeriod; otherwise, a sloppy behavior was to be expected.

I observed that this can be further improved as the behavior is still far from ideal when commands flow irregularly and in lower frequencies. I suggest a dual mechanism to achieve this:

• Signal filtering via a low-pass filter (IIR): Wikipedia (permalink). It can be actually seen as an exponential moving average (EMA). I recently implemented one here.
• Spline interpolation via higher-order polynomials, i.e. (beyond linear ones) cubic and perhaps even quintic polynomials. I'm also thinking of minimum jerk trajectory generators, see this YARP implementation for Gazebo: hpp, cpp.

[PeterBowman]

On a side note, the filtering and interpolation could be done by the device that is sending the commands to our joint controllers, instead of those.