Remove Singularities from Floating 6 DOF

[liangfok]

The current implementation uses 6 serial joints. This suffers from singularities. It should be switched to spherical joints. See controlit_model/src/rbdl_robot_urdfreader.cpp, lines 409-415.