Handle SO(2) trajectories

[brianhou]

We need to handle wrapping from SO(2) trajectories for ADA. There are (at least) two related TODOs:

• https://github.com/personalrobotics/rewd_controllers/blob/master/src/JointAdapter.cpp#L79
• https://github.com/personalrobotics/rewd_controllers/blob/master/src/JointAdapter.cpp#L121

I think two strategies would be to:

1. Keep track of the previous R1 state, and compute the correct delta
2. Immediately convert an SO(2) trajectory to an R1 trajectory when it's received in JointTrajectoryControllerBase::goalCallback

2 seems a little bit cleaner to me, but does anyone have any preference or other ideas?