Add SO(2) taskmap ContinuousJointPose, fix cost-indexing in dynamic problems, enable multi-modal solutions

[wxmerkt]

• Allows multi-modal solutions on the CartPole using either Point2Plane or a new taskmap to treat SO(2) (ContinuousJointPose)
• Fixes cost indexing #618
• Fixes URDF issue introduced in #622
• Resolves #617 by using a specialised SO(2) taskmap
• Adds whole-body, no-floating base planning group to Talos

cc: @include4eto - changing cartpole example to use Point2Plane taskmap - does not converge. We probably have to go with a new taskmap for the wrapped joint angle.