Bugfix: New way of handling constraints in 6-DOF