Started to add holonomic constraints to dynamics