Improve quality of motion by interpolating incoming trajectories @ 4ms

[gavanderhoorn]

This is already a TODO in the driver:

staubli_val3_driver/staubli_val3_driver/val3/ros_server/motionControl.pgx

Lines 43 to 44 in 17fd14a

	// process motion command from move buffer
	// @TODO move buffer should have elements previously interpolated to ensure they are 4 ms apart

The current implementation essentially forwards the incoming ROS trajectory 1-to-1 without any interpolation or post/preprocessing.

I'm wondering whether interpolation on the controller -- with a known velocity and acceleration curve -- could allow us to avoid situations such as #2 and ros-industrial/staubli_experimental#25.

motoman_driver does something similar here. While it potentially does not execute the exact same motion as the trajectory prescribes, it does make sure the trajectory timing/velocity/acceleration profiles fit with what works well with the robot.

[gavanderhoorn]

@marshallpowell97: would this be something you would deem as worth it? It would be a bit of an investment to implement this on the controller I guess.

It would not help make the driver interface any more dynamic (as requested in #20), but it may help with increasing (perceived) motion quality.

Note that this sort of functionality wouldn't even be ROS-specific: any interface used to transmit sparse trajectories to a Staubli controller could/would benefit from this.