Cleaning up things

  - Debug desired force
Merge branch 'rvizui' of github-vasquezg:srl-freiburg/pedsim-simulator into rvizui

Conflicts:
	src/scene.cpp
Merge branch 'rvizui'