Handle floating-bases properly

[wxmerkt]

Now that we are implementing dynamic problems, the incorrect handling of the floating-base is coming to bite us - it is not straight-forward to extract and propagate the sizes of the position and velocity vectors. However, this would likely mean redesigning the kinematics entirely - which is quite an undertaking.

• [ ] Move from KDL::Frame to Eigen::Isometry3d in all APIs, i.e., push KDL::Frame to within KinematicTree.
• [ ] Properly represent the SE(3)/SE(2) floating-base joints in KinematicTree. This may require changing to another kinematics library.
• [ ] Propagate actuated / model (includes fixed) / controlled sizes for position and velocity from a single place