Added logging to file. Currently logs joint pos.
Now logs joint velocity. Also added matlab script