More work on issue #35. Updating updateTrajectory.

Discarded the timestamps-only updateTrajectory and updated the original
one to update the time_from_previous waypoints in the RobotTrajectory
and the velocities and accelerations in every joint / waypoint.