Consistency in theta definitions, reduced arm DOF

Made sure all parts of the code were rotating consistently and changed
optimization code to begin search for thetas from last known thetas.
Reduced optimization time from 4 seconds to ~0.7 seconds. Needs to be
faster still.