Robot reaches an angle of contention

Tried leaving \theta unconstrained. Tried constraining \omega.  Tried
normalizing angles in different locations. (Fixed definition of beta)