Added a method to directly insert trajectory point --kazu