Refactored trajectory handler into its own file, set up safe parameter system, and created some unit tests for the utilities.