Fix contact force limit.
Add more tests and write robot configuration in a file.