Add command line arguments for setting test parameters
Move collision solving to dynamics step so the two robots envolved can detect the collision
Remove unused variables
Fix IR sensor bug (negative index (idx) causing vector underflow)