Merged Changes for Trajectory Following.
Merge branch 'Alex_Manipulator'
Added tool to robot transform function
Changed things so it will be easy to generate mod files on stdout.