Still working on issue #35.

When updating the "updateTrajectory" function to not use
trajectory_msgs::JointTrajectory I ended up in some code that seems old
and not longer in use.  I think that velocities and accelerations
calculated in this function are not used anywhere, that's why I created
a new updateTrajectory which just updates the time between waypoints
using the previously calculated time_diff vector.