Added proper SO2 handling for Spline conversion

[egordon]

Tested on entire feeding demo, both in simulation and on the real robot. Will run unit tests before merging.

To clarify the issue:

Since the Spline conversion was happening in R1, during wrap, the interpolation would actually add waypoints going the long-way around (i.e. if we send 2 waypoints pi and -pi as R1 over ROS, the reconstructed spline would go from -pi through 0 to pi, rather than just leaving them be).

This would trip the velocity constraints in rewd_controllers.

While there a a fix en route in rewd_controllers (personalrobotics/rewd_controllers#34), it makes sense to add a fix here too, in case the controller isn't as intelligent as, say, ros_trajectory_controller.

---

Before creating a pull request

• Document new methods and classes
• Format code with make format

Before merging a pull request

• Set version target by selecting a milestone on the right side
• Summarize this change in CHANGELOG.md
• Add unit test(s) for this change

[aditya-vk]

If I understand correctly, this PR is making sure we send in a trajectory the same type as the original statespace and not one in R as was done previously.

Left some comments to be addressed. The additions look sound to me.

[egordon]

@aditya-vk The point of this PR is the reverse. I don't touch the code that sends trajectories. Instead, this code takes an incoming R^n trajectory and re-interprets it as R^n+SO(2)^m based on the MetaSkeleton. It makes receiving more robust while leaving sending unaffected.

[gilwoolee]

@egordon would you mind clarifying the assumptions we have in ROS trajectories? (or the issue you were facing that led to this PR?) Is it that the ROS trajectories of SO2 joints are unwrapped into R1 space so that we need to do the inverse-compose-log to get it back to SO2 space diff? I thought we originally did not put this in because we expected the delta (nextPosition-currPosition in line 500) to be relatively small such that the original code would work.

In general, the PR should give a detailed description of the issue you're resolving. :)

[egordon]

@gilwoolee updated PR description.

Out-of-date.

[aditya-vk]

Majority of the PR seems to be formatting, it would have been clearer to separate that out. Anyway, could you clarify if the following is correct?

1. We plan in R+SO2
2. We convert it to Rn so we can time it appropriately. We get an SO2 spline out.
3. When we convert to ros trajectory (toROSJointTrajectory()) the trajectory seems to be converted back to R+SO2
   3a. since existing ROS trajectories handle SO2 spaces appropriately, we are fine there.
   3b. rewd controllers convert the ros trajectory back to a spline via toSplineTrajectory() here we currently do not account for SO2 joints and hence the spline conversion could go bad, this PR fixes this conversion.

Could you point to the issue fixed in rewd controllers related to this PR? [as mentioned in the PR description]

Looks good to me otherwise.

[aditya-vk]

Looks good to me! We can merge if the tests pass.

[aditya-vk]

LGTM. We can merge post Travis checks.