WIP: constraint_forces implemented, need to test.