improve walking by storing last collision free position and restoring it in...

improve walking by storing last collision free position and restoring it in case that is needed, also move out of dynamic collisions
closes #13