state lookup utility for robot_trajectory

An implementation of "get the robot state at some duration from the start of the trajectory".
This is what I use with my teleop stuff, so... it seems to work :)