Changed camera position.
Changed getFK to get transform of link instead of end effector transform.
Draws axes for multiple transforms.
Removed bug where grab/release indices were not being cleared for each segment of playback.
Fixed minor changes in the playback for grabbing and releasing.
Added a bunch of default variables to de-clog command line.
Major changes to the code in order to get John's TrajOpt code to work for us.
Merged code.