Port over ros_controllers interpolation formulas for joint_trajectory_controller

[ddengster]

As per title. Check the sample function in the Trajectory class.

This comment should be a good start. I think it also mentions it uses Horner's algorithm somewhere in there. Would be nice if we could avoid introducing another class to handle it.