Holonomic constraints added. DOES NOT PASS THE TEST YET.