Generalizing code that uses the holonomic constraints and renamed some variables and their order. Now have g(x, y, theta) and shape variables are in q and u.
Revisiting the code, and looking for mistakes. Also added some output of the mass and forcing vectors.