Main function now loads csv file as data.
Added Body Argument for parser to accept.
Added Test Mocap data
Updated 6640_test to include trajectory tracking.
Added Function for playing list of transforms (AKA output of getMocapTraj)
Changed environment to remove lines.
Merge branch 'Alex_Manipulator'