Improved the introduction of holonomic motion.
Using different weights in the qp for holonomic and nonholonomic.
Corrected errors.
Improved the border situation considering from/to holonomic motion.