Add first-class support for SE(2) and SE(3) state spaces

[mkoval]

We have SE(2) and SE(3) state spaces implemented in aikido::statespace, but they are not fully supported by the rest of Aikido (e.g. distance metrics, interpolation methods, DART conversions). We need to add this functionality to make them first-class citizens.

@aaronjoh, @lgw903, and I discussed this offline and concluded that it is not possible to represent SE(2) and SE(3) as CompoundStateSpaces of SO(2), SO(3), and a real vector space because the group operation differs. However, we may be able to gracefully handle both of these by implementing an AffineStateSpace or an semi-direct product space. We should investigate this idea further before proceeding with implementation.

---

TODOs

• [ ] SE(2) distance metrics
• [ ] SE(3) distance metrics
• [ ] SE(2) interpolation methods
• [ ] SE(3) interpolation methods
• [ ] SE(2) DART conversions
• [ ] SE(3) DART conversions

[jslee02]

I've added unverified list of TODOs. :sweat_smile: Please feel free to update the list and let's track what needs to be done.

[egordon]

Archiving